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Preface 


Currently,  the  F-4E/G  uses  a  Wiener-Hopf  filter  for 
estimating  target  position,  velocity,  and  acceleration  during 
air  combat  maneuvering.  As  implemented,  the  estimates 
contain  unacceptable  errors.  The  purpose  of  this  study  is  to 
determine  the  feasibility  of  replacing  the  Wiener-Hopf  filter 


with  a  Kalman  filter  in  order  to  obtain  better  estimates, 


The  determination  is  made  by  first  designing  an  appropriate 
Kalman  filter  and  then  testing  the  design  through  computer 
simulation . 
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Abstract 

Currently,  the  F-4E/G  uses  a  Wiener-Hopf  filter  for 
estimating  target  position,  velocity,  and  acceleration  during 
air  combat  maneuvering.  As  implemented,  the  errors  between 
the  actual  target  variables  and  the  estimate  of  these 
variables  are  too  large.  The  purpose  of  this  study  is  to 
evaluate  the  feasibility  of  replacing  the  Wiener-Hopf  filter 
with  a  Kalman  filter  in  order  to  obtain  better  estimates. 

The  evaluation  is  made  by  first  designing  an  appropriate 
preliminary  design  Kalman  filter  and  then  testing  the  design 
through  a  Monte  Carlo  computer  simulation  analysis.  The 
computer  simulation  results  indicate  that  the  Kalman  filter 
is  capable  of  significantly  outperforming  the  Wiener-Hopf 
filter,  and  as  such,  should  be  developed  into  a  final  design. 

The  Kalman  filter  contains  nine  states  (three  relative 
target  position,  three  total  target  velocity,  and  three  total 
target  acceleration  states).  Filter  propagation  is  based  on 
linear  time-invariant  dynamics  primarily  because  of  the 
limited  capabilities  of  the  on-board  aircraft  computer.  The 
linear  dynamics  permits  propagation  by  a  state  transition 
matrix.  Measurement  updates  use  six  measurements  (range, 
range  rate,  azimuth  angle,  elevation  angle,  azimuth  rate,  and 
elevation  rate)  available  on  the  F-4.  Both  continuous  time 
sampled-data  and  discrete-time  sampled-data  designs  are 
included. 


PRELIMINARY  KALMAN  FILTER  DESIGN  TO  IMPROVE 


AIR  COMBAT  MANEUVERING  TARGET  ESTIMATION 
FOR  THE  F-4E/G  FIRE  CONTROL  SYSTEM 

I.  INTRODUCTION 

1 . 1  Motivation:  Organizational  Problem 

There  are  more  than  3500  F-4  aircraft  in  service 
worldwide  today,  and  the  Pentagon  predicts  that  over  2000 
will  still  be  in  service  in  the  year  2000  (1:16-18).  This 
prediction  has  resulted  in  the  realization  that  the  F-4  will 
remain  in  the  USAF  inventory  much  longer  than  initially 
planned  (1:16-18).  As  a  consequence,  the  Air  Logistics 
Center  (ALC)  at  Hill  AFB  is  continually  upgrading  F-4E/G 
aircraft  systems  to  improve  aircraft  survivability  and 
effectiveness  in  hostile  zones  of  operation.  Improved 
capability  directly  influences  the  future  utilization  of 
F-4E/G  aircraft.  In  order  to  support  the  upgrade, 
OO-ALC/MMECB  has  requested  Air  Force  Institute  of  Technology 
(AFIT)  assistance  in  improving  F-4E/G  air  combat  maneuvering 
algorithms  for  target  estimation  (2:1-2).  An  improved  air 
combat  maneuvering  capability  will  increase  the  effectiveness 
of  this  aircraft  in  the  air-to-air  role  and  should  increase 
the  air  combat  survivability  of  the  fighter. 


1.2  Present  Situation 


In  an  air-to-air  role  an  aircraft  must  first  be  able  to 
maneuver  to  elude  the  enemies'  fire  power  and  second  be 
capable  of  delivering  its  own  fire  power.  The  overall 
objective  is  to  be  able  to  launch  a  missile  against  a  target 
when  the  probability  of  kill  is  high  (most  USAF  F-4s  do  not 
have  guns  and  they  rely  primarily  on  air-to-air  missiles  for 
fire  power).  However,  when  F-4E/G  aircraft  maneuver  during 
an  air-to-air  engagement,  the  fire  control  system  which 
predicts  when  the  target  is  in  the  envelope  of  vulnerability 
becomes  unstable.  As  currently  implemented  on  the  F-4E/G, 
large,  unacceptable  errors  in  target  position,  velocity,  and 
acceleration  estimates  result.  Pilots  who  fly  the  F-4E/G 
models  have  complained  that  the  target  steering  dot 
oscillates  and  is  distracting,  especially  during  high  roll 
rate  maneuvers  (3:1).  As  a  partial  solution,  a  post  filter 
has  been  added  to  detect  when  the  plane  starts  to  roll  and 
then  "fixes"  the  steering  dot  in  place.  This  results  in  the 
steering  dot  sometimes  "jumping  around"  on  the  pilot's 
display  after  the  dot  is  released  from  the  "fixed"  position. 
Thus,  the  air  combat  maneuvering  target  information  provided 
to  the  pilot  is  of  questionable  value  and  may  result  in 
launching  a  missile  outside  its  effective  envelope  and 
consequently  missing  its  target.  As  such,  when  the  pilot 
needs  target  information  most,  the  steering  dot  may  either  be 
oscillating,  fixed  in  place,  or  not  used  because  of  pilot 
distrust . 


viv.v: 


OO-ALC/MMECB  engineers  attribute  the  unstabLe  nature  of 
the  steering  dot  to  "noisy  and  inaccurate  line-of-sight 
rates"  and  the  use  of  a  Wiener-Hopf  filter  for  providing 
target  estimates  (2:1-2).  It  may  be  possible  to  stabilize 
the  steering  dot  by  either  replacing  the  radar  system  with  an 
improved  system,  or  by  replacing  the  Wiener-Hopf  filter  with 
a  Kalman  filter,  or  both.  The  USAF  is  not  willing  to  request 
or  provide  funds  to  update  the  radar  system  (1:16-18).  Thus 
at  the  present  time,  the  only  viable  solution  is  to  develop 
and  test  a  dynamic  software  filter  in  order  to  try  to 
eliminate  or  significantly  reduce  the  distracting  steering 
dot  oscillation  and  jump.  This  solution  is  a  low  cost  option 
because  it  does  not  involve  hardware  changes. 

Better  estimates  may  theoretically  be  obtainable  by 
developing  adequate  state  dynamic  models,  measurement  models, 
time  propagation  models,  and  measurement  update  equations  and 
then  incorporating  these  into  a  properly  tuned  Kalman  filter. 
However,  an  actual  history  of  radar  system  noises  and 
measurement  noises  is  not  accurately  known.  Without  actual 
noise  data,  it  is  not  possible  to  tune  a  Kalman  filter  for 
implementation.  The  tuned  filter  in  this  thesis  is  based  on 
values  of  actual  noises  from  one  flight  test  (nonmaneuvering 
aircraft  and  target)  and  truth  model  dynamic  radar  lags.  As 
such,  a  sensitivity  analysis  based  on  simulation  techniques 
for  various  types  of  noises  is  desirable  for  follow  on 
testing  in  OO-ALC  test  facilities  and  test  aircraft. 


1 . 3  F-4E/G  System  Limitations 

The  F-4E/G  fire  control  system  is  not  a  state-of- 
the-art  system,  which  places  constraints  on  the  design  of  an 
Kalman  filter.  The  limitations  are: 

1.  The  fire  control  computer,  the  LRU-1,  is  a  fixed 
point,  16  bit  wordlength  machine. 

2.  The  analog  to  digital  convertors  provide  only  10 
significant  bits. 

3.  The  LRU-1  operates  at  300,000  operations  per  second. 

4.  The  total  memory  in  the  LRU-1  which  can  be  allocated 
for  the  target  estimation  is  8K  words  which  must  be 
shared  with  a  long  range  intercept  (LRI)  algorithm 
(concurrently  being  modified  in  another  thesis  (4)). 

5.  Target  estimation  update  is  currently  required  every 
40  msec.  An  update  period  range  between  40  and  100  msec 
is  required. 

The  fixed  point  and  analog  to  digital  conversion 
restrictions,  as  well  as  other  restrictions,  impact  on  the 
overall  design  philosophy.  The  operating  time  and  memory 
restrictions  imply  the  need  for  efficient  yet  accurate 
reduced  order  model  Kalman  filters.  The  memory  restrictions 
led  to  an  early  realization  that  air  combat  maneuvering  and 
long  range  intercept  (a  parallel  Air  Force  Institute  of 
Technology  Thesis  (4))  algorithms  must  be  shared  to  make 
efficient  use  of  available  memory.  For  the  design  to  be 
useful  in  the  real  world,  simulation  testing  must  account  for 
the  above  restrictions. 


The  primary  emphasis  of  this  study  is  to  design  and 
test  Kalman  filter  algorithms  for  estimating  target  position, 
velocity,  and  acceleration  to  determine  the  feasibility  of 
replacing  the  current  Wiener-Hopf  filter  with  a  Kalman  filter 
for  F-4E/G  air  combat  maneuvering.  Air  combat  maneuvering, 
for  this  thesis,  is  defined  as  air-to-air  combat  below  32,000 
feet  of  elevation  and  within  an  8  nautical  mile  radius  of  the 
F-4  aircraft.  The  intent  is  first  to  provide  sufficient 
theory  on  which  a  number  of  Kalman  filters  can  be  designed 
for  this  particular  problem.  Then,  to  propose  a  nine  state 
reduced  order  Kalman  filter  as  a  preliminary  design 
considering  the  system  restrictions  described  in  Section  1.3. 
Next,  the  preliminary  design  Kalman  filter  is  tested  through 
computer  simulation  to  validate  its  performance.  Finally, 
recommendations  are  made  on  areas  where  further  research  may 
be  warranted.  Every  attempt  is  made  to  model  or  account  for 
the  system  restrictions  so  the  final  product  will  be  of  use 
on  real  world  F-4E/G  aircraft. 

1 . 5  Scope  and  Organization 

This  study  follows  a  systematic  design  procedure  which 
parallels  a  procedure  proposed  by  Maybeck  (5:341-342).  Due 
to  the  limited  time  available  for  this  effort,  it  is  not 
possible  to  complete  in  full  detail  all  of  the  steps  of  the 
systematic  design  process.  The  procedure  is  included  to 
illustrate  the  design  approach  employed.  The  procedure  is  as 


follows  (with  responsibilities  defined). 

1.  Development  of  a  "truth  model"  based  on  the  system 
dynamics  and  measurement  models  (this  study,  Chapters 
II,  III,  and  IV  and  trajectory  simulation  program  from 
OO-ALC ) . 

2.  Development  of  the  Kalman  filter  theory  based  upon 
the  "truth  model"  (this  study,  Chapter  III). 

3.  Proposal  of  a  simplified,  reduced  order  Kalman 
filter  based  on  system  models  and  F-4  E/G  system 
limitations  (this  study,  Chapter  III). 

4.  Development  of  test  trajectory  algorithms  (this 
study,  Chapter  IV). 

5.  Completion  of  a  Monte  Carlo  analysis  (sensitivity 
analysis)  on  the  selected  reduced  order  Kalman  filter 
proposed  (this  study,  Chapter  V). 

6.  Completion  of  a  thorough  Monte  Carlo  analysis  based 
on  designs  showing  the  most  promise  (either  future  study 
or  OO-ALC/MMECB) . 

7.  Completion  of  a  performance  /  computer  loading 
tradeoff  analysis  and  selection  of  a  design  (partially 
this  study,  Chapter  IV,  Stand-Alone  Simulation  (SAS) 
Program,  and  Appendix  F:  future  research  or  efforts  by 
OO-ALC  warranted).  The  design  included  in  the  SAS  will 
be  proposed  to  OO-ALC  for  possible  implementation. 

8.  Implementation  of  the  chosen  designs  on  the  online 
computer  used  in  the  F-4E/G  (OO-ALC). 

9.  Completion  of  checkout,  final  tuning,  and 


operational  tests  of  the  filter  (OO-ALC). 

1 .6  Literature  Review 

As  a  service  to  the  reader  and  OO-ALC,  the  contents  or 
abstracts  and  important  aspects  of  many  references  pertaining 
to  this  study  are  listed  in  Appendix  A. 


II.  ANALYTICAL  DEVELOPMENT  (MODELS) 


2. 1  Introduction 

This  chapter  contains  the  necessary  concepts,  geometry, 
and  system  equations  for  mathematically  modeling  an  air-to- 
air  engagement  for  implementation  on  a  digital  computer.  The 
models  in  this  chapter  are  specific  for  the  F-4E/G  but  can 
easily  be  adapted  to  a  wide  class  of  problems.  As  an  example 
of  the  specific  design,  the  radar  model  is  for  a  space 
stabilized  gimballed  radar;  strapdown  models  are  not 
addressed,  but  can  be  found  in  other  references  (6:23-32). 

The  overall  intent  is  to  provide  the  necessary  background, 
system  restrictions,  assumptions,  and  data  required  so  that  a 
reader  with  air-to-air  scenario  background  can  logically 
follow  the  development.  To  accomplish  this,  the  chapter  is 
divided  into  the  following  subsections:  coordinate  system, 
radar  description,  dynamics  model,  simulated  measurement 
model  (radar),  and  the  truth  model. 

2.2  Coordinate  System,  Basic  Concepts 

The  air-to-air  engagement  can  be  modeled  in  either  a 
Cartesian  coordinate  frame  or  a  spherical  coordinate  frame. 
The  Cartesian  coordinate  frame  is  referenced  to  a  flat  earth 
approximation  and  the  spherical  coordinate  frame  is 
referenced  to  the  line-of-sight  (1)  or  the  tracker  frame. 

The  choice  is  arbitrary,  but  impacts  on  both  the  measurement 
model  (radar)  and  the  system  dynamics  model.  A  Cartesian 
coordinate  system  is  used  throughout  this  thesis  for  the 


reasons  discussed  below. 

Using  a  Cartesian  coordinate  frame  (located  at  the 
aircraft  center  of  gravity  (c.g.),  flat  earth  approximation) 
results  in  a  linear  model  for  target  dynamics.  The  angular 
rate  terms  which  appear  in  differential  equations  written  in 
a  rotating  frame  are  eliminated.  In  other  words,  the 
Coriolis  and  centripetal  acceleration  terms  are  eliminated 
from  the  system  model.  A  slight  variation  is  a  Cartesian 
reference  frame  that  rotates  about  the  c.g.  (i.e.,  moving  the 
origin  of  the  frame  from  aircraft  center  of  gravity  to  radar 
tracker  frame).  The  distance  from  the  c.g.  to  the  tracker 
frame  and  rotation  rates  in  velocity  and  acceleration 
estimates  can  be  accounted  for  by  applying  dynamic  equations 
and  relations.  Linear  target  dynamics  are  retained.  Radar 
units  are  typically  in  the  nose  of  the  fighter  aircraft  (not 
the  center  of  gravity),  thus  the  radar  is  often  modeled  in  a 
Cartesian  frame  located  a  fixed  distance  from  the  c.g.  This 
is  the  frame  used  throughout  this  study.  Figure  2-1 
illustrates  the  antenna  tracker  frame  used. 

When  linear  target  dynamics  are  preserved,  it  is 
possible  to  form  a  state  transition  matrix  resulting  in  an 
easier  and  more  computational  efficient  digital 
implementation.  The  price  one  must  pay  for  this  simpler 
dynamic  system  is  a  nonlinear  measurement  (radar)  model. 

A  spherical  reference  frame,  on  the  other  hand,  results 
in  linear  measurement  models  but  nonlinear  target  dynamics 
equations.  For  the  air-to-air  problem,  the  nonlinear  dynamic 
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Figure  2-1.  Antenna  Tracker  Reference  (i^.j^.k  )  and  Antenna 
Coordinates  (i,j,k)  °  °  ° 

(Adapted  from  T.O.  12P2-2APQ120-2-3-7 ) 


equations  result  in  nonlinear  propagation  equations  which 
require  on-line  integration.  Conversely,  the  linear, 
time-invariant  dynamic  equations  associated  with  the 
Cartesian  coordinate  frame  result  in  algorithms  that  do  not 
require  on-line  integration  to  compute  the  state  transition 
matrix  (7:1-28).  Therefore,  the  target  dynamics  are  usually 
modeled  with  respect  to  a  Cartesian  coordinate  frame,  while 
the  radar  measurements  typically  consist  of  at  least  vehicle 
slant  range,  azimuth,  and  elevation  (7). 

Without  loss  of  generality,  a  Cartesian  coordinate 
system  is  used  in  following  subsections  on  development  of 
radar  models  and  system  dynamic  models.  This  is  consistent 
with  the  current  F-4E/G  filter  design,  since  the  P004  update 
documentation  states,  "critical  measurements  of  target 
position  and  velocity  relative  to  the  F-4E/G  are  made  in 
radar  antenna  coordinates,  it  would  be  very  convenient  to 
express  the  filter  inputs... in  this  same  triad"  (8:3-423). 

As  such,  the  primary  reference  frame  is  the  antenna  tracker 
reference  frame. 

2 . 3  Radar  Description 
2.3.1  Introduction 

The  study  of  radar  can  be  a  vast  and  complicated 
process;  a  thesis  all  in  itself.  00-ALC/MMECB  has  developed 
a  radar  simulation  model  which  is  modified  and  used  in  this 
thesis  (see  Chapter  IV  and  Appendix  B).  The  intention  of 
this  section  is  to  develop  and  discuss  only  the  necessary 


radar  concepts  and  information  to  understand  the  quality  and 
quantity  of  the  radar  inputs  to  the  F-4E/G  fire  control 


system.  The  characteristics  of  the  measurement  noise,  v(t^) 
are  established  for  the  radar  measurement  equation  (see 
Section  3.2.1  for  further  explanation)  as 

z(t.)  =  h[x(ti),ti]  +  v(tt)  (2-1) 

where 

z(t^)  =  noise  corrupted  vector  measurements  at  time  t^, 

h[x(t.),t.]  =  nonlinear  measurement  function,  and 

v( t^ )  =discrete  zero  mean  white  Gaussian  noise  at  time 
with  covariance  R ( t ^  ) . 

2.3.2  Radar  System,  Basic  Concepts 

In  simplistic  terms,  the  F-4E/G  APQ-120  radar  is  a 
system  which  transmits  an  electromagnetic  waveform  and 
receives  back  a  reflected  portion  of  the  waveform.  From  this 
reflected  waveform,  the  system  provides  measurements  of 
range,  range  rate,  azimuth,  elevation,  azimuth  rate,  and 
elevation  rate.  The  measurements  are  relative,  from  the 
apparent  radar  centroid  of  the  target  to  the  attacker  F-4 
radar.  The  information  can  be  represented  in  either 
Cartesian  or  line-of-sight  reference  frames,  but,  as 
discussed  in  Section  2.2,  this  study  utilizes  the  Cartesian 
frame.  Additionally,  the  F-4E/G  radar  is  a  non-Doppler, 
gimballed,  air  intercept  (AI),  space  stabilized  system.  The 


radar  antenna  rotates  about  a  fixed  point  in  the  aircraft 
relative  to  a  radar  reference  frame  i  ,  i  ,  and  k  (see 
Figure  2-1).  Rate  gyros  and  resolvers  on  the  radar  antenna 
provide  azimuth  and  elevation  rates  and  azimuth  and  elevation 
angles,  respectively,  to  the  radar  reference  frame. 
Functionally,  once  a  target  is  detected  in  a  search  mode,  the 
aircrew  or  the  radar  system  itself  can  lock  on  and  track  the 
target,  providing  the  measurements  listed  above.  Then  the 
measurements  are  used  by  the  fire  control  system  to  compute 
when  a  target  is  in  range  for  firing  a  missile  against  the 
target.  In  a  practical  sense,  one  major  problem  remains: 
determining  how  good  or  how  accurate  the  measurements  really 
are . 

2.3.3  Radar  System,  Measurement  Corruption 

Radar  measurement  noises  degrade  the  quality  of 
information  provided  to  the  fire  control  system.  Since  the 
measurements  are  used  in  estimating  target  position, 
velocity,  and  acceleration,  the  noises  must  be  addressed. 

The  noise  sources  are  briefly  discussed;  the  intent  is  to 
only  provide  representative  noise  values  for  the  available 
F-4E/G  radar  measurements:  range,  range  rate,  azimuth, 
elevation,  azimuth  rate  and  elevation  rate.  According  to 
Barton  and  Ward  (9:Chapter  8),  radar  noise  can  arise  from  a 
large  number  of  sources.  Table  II-l  lists  common  noise 
sources  (refer  to  Barton  and  Ward  for  definition  of  the  noise 


sources ) . 


Table  II-l 


Common  Radar  Noise  Sources 

Angle  Error 


Thermal  Noise 
Clutter  and  Interference 
Multipath  Reflections 
Target  Glint  and  Scintillation 
i  Quantization  and  Array  Error 
|  Dynamic  Lag 

!  Atmosphere  Propagation 

! 

I  Monopulse  Network  Error 

;  Servo  and  Mechanical  Error 

Receiver  Time  Delay  Instability 

Time  Discriminator  Alignment  and 
Stability 


X 

X 

X 

X 

X 

X 

X 

X 

X 


;  Servo  Loop  Noise 

1 

i  Error  in  Converting  Measured  Delay 
to  output  data  (apart  from 
quantizing  noise) 

Reference  Oscillator  Frequency 
j  Stability 

i 


Range  Error 
X 
X 
X 
X 
X 
X 
X 

X 

X 

X 

X 

X 


Quite  often,  radar  noises  are  modeled  as  being 
dominated  by  just  the  noise  from  glint  and  scintillation 
(scintillation  is  commonly  called  amplitude  distribution 
(9:171)).  For  example,  the  last  update  for  the  OFP  ACM 
Computer  Modification  (8:3-413)  states  "the  major  sources  of 


measurement  noise  in  target  velocity  are  radar  generated 
glint  and  amplitude  noise  sensed  by  the  antenna  mounted  rate 


gyros."  However,  calculations  from  this  study  indicate  that 
additional  noise  sources  for  the  F-4E/G  radar  system  are 
introduced  through  antenna  dynamics  and  the  analog  to  digital 
(A/D)  conversion  (quantization  in  Table  1 1— 1 ) .  Antenna 
dynamics  are  discussed  in  Section  2.5.2.  The  A/D  conversion 
uses  only  10  significant  bits.  This  results  in  the  least 
significant  bit  (LSB)  adding  noise  to  the  measurement.  The 
added  noise  can  be  calculated  as  follows 


LSB  errors 


Maximum  Measurement  Interval  Size 


2 


10 


(2-2) 


Modeling  the  LSB  error  as  an  uniform  random  variable,  mean 
and  standard  deviation  formulas  for  both  a  truncated  and 
rounded  A/D  conversion  process  are  provided  in  Table  II-2 
(see  (5:92-93)  for  a  derivation).  Using  these  relations, 


Table  II-2 


Mean  and  Standard 

Deviation  Equations  for  A/D  Process 

mean 

standard  deviation 

Truncated  Case 

LSB/2 

LSB/(12)1/2 

Rounded  Case 

0 

LSB/ ( 1 2 ) 1 ^  2 

LSB  errors  are  illustrated  in  Table  1 1  —  3  (using  maximum 
measurement  values  from  F-4  Improved  Air-to-Air  Missile 
Program  (10:4-3)).  A  Gaussian  approximation)  (using  the  mean 
and  standard  deviation)  of  the  noise  process  introduced  by 
the  A/D  process  is  used  in  forming  R.nom  in  Equation  (2-3). 


Table  I 1-3 


Mean  and  Standard  Deviation  for  A/D  Process 


LSB  Error 

mean1 

mean^ 

S.D. 

Range( <60,000  feet) 

58.59 

29.29 

0 

16.91 

Range  Rate( ft/sec) 

1.93 

0.97 

0 

0.56 

Azimuth( radians ) 

0.00127 

0.00064 

0 

, 

0.000366 

(degrees ) 

0.0728 

0.0364 

0 

0.0210 

Elevation( radians ) 

0.00127 

0.00064 

0 

0.000366 

(degrees ) 

0.0728 

0.0364 

0 

0.0210 

Azimuth  Rate ( rad/sec ) 

0.000511 

0.00256 

0 

0.00148 

(deg / sec ) 

0.0293 

0.01465 

0 

0.00846 

Elevation  Rate(rad/sec 

)  0.000511 

0.00256 

0 

0.00148 

(deg/sec 

)  0.0293 

0.01465 

0 

0.00846 

1-  mean  for  the  truncated  case 

2-  mean  for  the  rounded  case 


Without  a  sufficient  history  of  radar  noise  data, 


nominal  values  are  selected  based  on  data  from  00-ALC/MMECB 


(11),  previous  studies,  and  the  above  A/D  noise  data.  The 
nominal  values  are  presented  in  Table  II-4  and  in  matrix  form 
in  Equation  ( 2-3 ) . 


Table  II-4 


Nominal 

Noise  Values 

for 

the  R  Matrix 

Measurement 

Symbol 

Standard  Deviation 

Range 

R 

17.0  feet 

Range  Rate 

RDOT 

16.0  ft/sec 

Azimuth  Angle 

AZ 

2.27  mrad 

Elevation  Angle 

el 

2.27  mrad 

Azimuth  Rate 

AZDOT  or 

wk 

12.22  mrad/sec 

Elevation  Rate 

ELDOT  or 

Wj 

12.22  mrad/sec 

289 

0 

0  0  0 

0 

0 

256 

0  0  0 

0 

R _ = 

0 

0 

5 . 15xl0~6  0  0 

0 

— nom 

0 

0 

0  5.15xl0-6  0 

0 

0 

0 

0  0  1.49xl0“4 

0 

0 

0 

0  0  0  1 

.49x10 

2.3.4  Selected  Radar  Functions 

The  development  to  this  point  considers  a  space 
stabilized  radar  that  is  locked-on  and  tracking  a  target 
providing  noise  corrupted  measurements  (minus  antenna 
dynamics  which  are  added  in  Section  2.5.2)  which  are 
representative  of  the  F-4  radar.  A  radar  simulation  model 
obtained  from  OO-ALC/MMECB  (12)  includes  the  F-4E/G  APQ-120 
radar  servo  dynamics  and  provides  outputs  of  measurements  of 
range,  range  rate,  antenna  azimuth,  antenna  elevation, 
azimuth  rate,  and  elevation  rate.  The  outputs  are  used  as 
inputs  to  a  Kalman  filter.  The  radar  servo  model  includes 
radar  antenna  dynamics  and  closed-loop  control  to  provide 
tracking.  The  only  assumptions  made  are:  1)  at  the  start  of 
a  simulation  run,  the  radar  is  already  locked  onto  the 
target,  and  2)  the  radar  tracks  the  target  (within  gimbal 
limits  of  plus  or  minus  60  degrees)  during  the  simulation. 

Using  the  00-ALC  radar  model,  with  noise  strengths  from 
Table  II-4,  it  is  now  possible  to  formulate  a  measurement 
model.  Since  this  model  is  based  on  the  target  estimation 
state  vector,  it  is  necessary  to  develop  the  target 
estimation  filter  geometry  and  dynamics  model. 

2.4  System  Dynamics  Model  (Radar  Reference  State  Equations) 
2.4.1  Introduction 

A  system  dynamics  model  is  required  to  obtain  position, 
velocity,  and  acceleration  estimates  of  target.  In  previous 
sections,  it  is  stated  the  radar  is  space  stabilized.  Noise 


corrupted  measurement  of  range,  range  rate,  azimuth  angle, 
elevation  angle,  azimuth  rate,  and  elevation  rate  are 
available.  As  discussed  in  Section  2.2,  it  is  desired  to 
develop  models  in  a  Cartesian  frame  resulting  in  linear 
dynamic  equations.  For  the  tracking  problem,  it  is  possible 
to  develop  a  linear  time-invariant  dynamics  model  which 
allows  efficient  implementation  through  a  state  transition 
matrix . 

2.4.2.  Geometry.  Coordinates,  and  Transformations 


Using  a  Cartesian  frame,  it  is  now  possible  to  describe 
the  problem  geometry,  assign  a  coordinate  system,  and  develop 
coordinate  transformations  from  one  frame  to  another. 
Descriptive  illustrations  (Figures  2-2  through  2-6)  are  used 
to  facilitate  the  discussion. 

The  geometry  and  notation  is  consistent  with  the  F-4E/G 
P004  update  (8:Chapter  3).  Euler  rotations  of  yaw  (I), 
pitch  (0),  and  roll  {4>)  are  employed.  Figure  2-2 
illustrates  aircraft  (l,m,n),  space  (x,y,z)  and  geographic 
(N,E,D)  frames  with  appropriate  Euler  angles.  Note  that  the 
geographic  and  space  frames  differ  by  only  a  rotation  in 
heading.  Further,  note  that  the  aircraft  frame  is  obtained 
by  successive  rotations  of  pitch  and  roll  from  the  space 
frame.  Figure  2-3  also  ilustrates  this  process. 

To  be  consistent  with  the  F-4E/G  aircraft,  a  two  degree 
offset  in  pitch  is  included  between  the  aircraft  (l,m,n)  and 
radar  reference  ( iQ , jQ ,  1c  )  frames.  Figure  2-4  illustrates 


& 


*1  »** 


fl 


PITCH  ATTITUDE 


-  m 


ROLL  ATTITUOE 


Figure  2-3  Space  and  Aircraft  Coordinates 
(Adapted  from  T.O.  12P2-2APQ120-2-3-7 ) 
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Next,  Figure  2-5  illustrates  Euler  rotations  of  azimuth 

(A  )  and  elevation  (E,  )  from  the  radar  reference  frame 
z  i 

(1o»  W  t0  ra^ar  tracker  frame  (i,j,k).  Additionally, 

Euler  rotations  of  azimuth  error  (A  )  and  elevation  error 

ze 

(E^g)  around  the  radar  tracker  frame  are  illustrated  to 
obtain  the  line-of  sight  (i^,j^,k^)  frame.  Azimuth  and 
elevation  errors  are  included  because  of  the  radar 
measurement  corruption  noise  discussed  in  Section  2.3.3. 
Figure  2-6  also  illustrates  these  error  angles. 

Given  the  geometry,  coordinate  transformations  are  now 
defined.  Starting  with  the  geographic  frame  and  multiplying 
by  the  rotation  matrices  in  the  order  shown  in  Equation 
(2-4),  the  transformation  from  the  geographic  frame  to  the 
line-of-sight  (1)  frame  is  determined.  Also  note  that  the 
frames  as  well  as  the  notation  symbols,  i.e.,  l,m,n  for  the 
aircraft  frame,  are  included  in  Equation  (2-4). 


i  N 

j  =  [Ele]  [Aze]  [El]  [Az]  [ -2 ' ]  u>]  [0]  [I]  E  (2-4) 

k  |  |  D 

J1  I  I  L  1 

1  TRACKER  RADAR  AIRCRAFT  SPACE  GEOGRAPHIC 
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ffl=  Aircraft  heading  angle, 

0=  Aircraft  pitch  angle, 

Aircraft  roll  angle, 

-2°  =  Radar  offset  from  aircraft  reference  (i  versus  iQ), 
A^  Radar  azimuth  angle, 

E^=  Radar  elevation  angle, 


A^£=  Azimuth  error  angle  between  radar  and  1  frames, 

E^e=  Elevation  error  angle  between  radar  anu  1  frames, 

N=  North, 

E=  East, 

D=  Down, 
c=  cosine,  and 
s  =  sine 

To  reverse  this  process,  i.e.,  to  obtain  N,E,  and  D  from  the 
1  frame  or  i^,j^,  and  k^,  Equation  (2-5)  is  employed. 


N  rp  rp  rp  rp  m  m  rp  rp  L  1 

E  =  [ffl]T[e]T[<6]T[-2°  ]T[A7]T[Et  ]t[a7F]t[e.  f]t  j} 

D  ^  L  zt  Lt  ki  (2_5) 


rp  rp 

where  [•]  is  the  transpose  of  [•].  (Note  that  [•]  can  be 
used  instead  of  [•]“*  (inverse)  because  the  transformation 
matrices  are  orthogonal  and  the  transpose  equals  the 
inverse.)  Since  the  tracker  is  space  stabilized,  it  is 
possible  to  determine  the  tracker  attitude  with  respect  to  an 
non-rotating  reference  frame  provided  by  the  tracker's  rate 
gyros  and  resolvers.  The  tracker  frame  is  a  fixed  distance, 
d,  from  the  fighter  center  of  gravity  (a  Cartesian  reference 
frame)  and  rotates  about  the  center  of  gravity.  Figures  2-1 
and  2-4  illustrate  the  geometry  of  the  tracker.  The  tracker 
frame  orientation  is  obtained  from  Euler  angle  rotations  0 
and  <t> ,  the  two  degree  rotation  factor,  A^,  and  E^, 
respectively,  if  the  xyz  is  considered  the  reference  frame. 
Denoting  xyz  frame  as  the  reference  frame  I,  and  the  tracking 


frame  as  T,  the  coordinate  transformation  matrix  is 

defined  as  the  transformation  matrix  from  I  coordinates  into 
T  coordinates 

CT/I  =  [El]  [Az]  [-2°]  [«n  [0]  (2-6) 

Further,  denoting  the  line  of  sight  frame  as  1,  the 
transformation  from  the  tracker  frame  to  the  1  frame  is: 


C1/T  “  tELE^  [AZE] 


(2-7) 


Finally,  the  transformation  from  the  reference  frame  to  the  1 
frame  is: 


1/1  =  Cl/T  pT/I 


(2-8) 


2.4.3  State  Equations  and  State  Transition  Matrix 

Cartesian  components  of  relative  position,  total 
velocity,  and  total  target  acceleration  are  used  in 
describing  the  tracking  state  equations.  The  states  are  as 
follows  (see  Figure  2-7): 


x^  =  relative  x  distance  between  the  tracker  reference 
frame  and  the  target  c.g. 

*2  =  total  target  x  velocity  coordinatized  in  the  tracker 
reference  frame. 

x^  =  total  target  x  acceleration  coordinatized  in  the 
tracker  reference  frame. 


x,  -  relative  y  distance  between  the  tracker  reference 


frame  and  the  target  c.g. 

x^  =  total  target  y  velocity  coordinatized  in  the  tracker 
reference  frame. 

x^  =  total  target  y  acceleration  coordinatized  in  the 
tracker  reference  frame. 

Xj  =  relative  z  distance  between  the  tracker  reference 
frame  and  the  target  c.g. 

Xg  =  total  target  z  velocity  coordinatized  in  the  tracker 
reference  frame. 

Xg  =  total  target  z  acceleration  coordinatized  in  the 
tracker  reference  frame. 

The  above  description  agrees  with  the  geometry 
established  in  Sections  2.2  and  2.4.2.  Also  note  that  the 
realtive  position  states  forces  the  magnitudes  of  the 
position  state  estimates  to  a  minimum  when  compared  to  total 
target  position  states.  This  limits  a  scaling  problem 
between  state  estimates  that  can  develop  when  total  target 
position  is  used  in  the  formation  of  the  state  vector. 


Target 

Centroid 


Figure  2-7  Fighter  to  Target  Position  States 


As  discussed  in  Section  2.2,  a  linear  system  dynamics 
model  is  desired.  A  first  order  Gauss-Markov  acceleration 
model  is  selected  because  it  allows  the  system  model  to 
remain  linear.  Higher  order  models  such  as  a  constant  turn 
rate  acceleration  model  (see  (13)  and  (14)  for  further 
explanation)  result  in  a  nonlinear  system  dynamics  model. 
Thus,  the  first  order  Gauss-Markov  acceleration  model  yields 
the  state  space  model: 

( t )  =  X2 ( t )  -V£x( t ) 

*2(t )  =  x3(t) 

*3(t)  =  (-l/rx)x3(t)  +  wx(t) 

*4(t)  =  x5(t)  -  vfy(t)  (2-9 

*5( t )  =  x6(t) 

x6(t)  =  (-l/ry)x6(t)  +  Wy(t) 

Xy ( t )  =  Xg( t )  -  Vjz( t ) 

Xg(  t  )  =  Xg( t ) 

Xg(t)  =  (-l/Tz)Xg(t)  +  Wz(t) 
where , 

vf  \r  velocity  of  fighter  in  x,y,  and  z. 

Lx  j  y  s  ^ 

r  ,  =  time  constants,  tau,  for  acceleration  models, 

x  9  y ,  z 

w  *  dynamic  noise  driving  the  acceleration  random 

x  ,y ,  z 

process  for  x^,  x^,  and  x^,  respectively. 

In  matrix  form,  after  dropping  the  time  argument,  the 
above  equations  are  represented  as: 


which  is  of  the  form 


x  =  Fx  +  Bu  +  Gw 


(2-11) 


The  state  transition  matrix,  (D(t,t  ),  for  this  time- 

—  o 

invariant  system  model,  is  defined  as 

ffl(t,tQ)  =  £-1[s_I  -  F]"1  (2-12) 

where,  1 

£  ^  =  inverse  Laplace  transform 
s  =  Laplace  operator 
1^  =  identity  matrix 

F  =  as  defined  in  Equations  (2-10)  and  (2-11) 

Performing  this  operation  results  in  a  block  diagonal  matrix 
of  the  form 


®<t.+1,t.)  =  ffl(t) 


®  '  0  1  0 
r~H - ! — 

°-aW- 

0  |  0  I  — z 


(2-13) 


where,  each  element,  ®^,i=x,y,z,  is  a  3x3  matrix  with 


=i 


1  At 
0  1 

0  0 


Ti2[(l/Ti)At-l+e'(1/Ti)At] 

T1[l-e“(1/Ti)At] 

e‘(1/Ti)At 


(2-14) 


J  i-x,y,z 


At  =  the  time  interval  (sample  period)  from  one  update 


•»  ^  •  **  « 
V  ■> 


i 


until  the  next,  and 
=  acceleration  time  constant,  i=x,y,z 

2.4.4  State  Equations  Translated  to  the  C.G 


As  discussed  in  Section  2.2,  the  antenna  tracker 
reference  frame  or  radar  reference  frame  is  the  primary 
reference  frame  used  in  this  thesis.  The  radar  reference 
frame  is  offset  a  distance,  d,  from  and  rotates  about  the 
c.g.  reference  frame.  As  such,  it  is  sometimes  desirable  to 
perform  calculations  in  the  c.g.  Cartesian  frame  since 
Coriolis  and  centripetal  acceleration  terms  equal  zero. 
Although  not  used  explicitly  in  this  thesis,  this  section  is 
included  for  completeness. 

Defining  d  as  (body  or  aircraft  frame) 


(2-15) 


1 ,  m ,  n 


and  describing  d  in  x,y,z  (c.g.  Cartesian)  coordinates  gives 


d  ( c© )  T  rp  d 

d  =  0  =  [0]T[tfjT  0 

-d(s0)  0 

*-  -*x,y,z  *-  -*lmn 


(2-16) 


Then  the  position  states  at  the  c.g.  are: 


xlcg  ‘  X1  +  d(c0) 


X,.__  =  XA 


(2-17) 


Next,  w  is  defined  in  the  c.g.  Cartesian  frame  as: 


ffi 


x,y,z 


(2-18) 


The  velocity  states  in  the  c.g.  frame  are  found  by  forming 
the  cross  product  wxd  and  adding  it  to  the  velocity  in  the 
tracker  state  (by  the  Corolis  theorem) 


(-x,: 


x  (d  ) 

-x , y , z 


-0d(s0) 

®d(c0)  +  ^d(se) 
-DId(c©> 


(2-19) 


The  velocity  states  at  the  c.g.  are: 

X2cg  =  X2  -  0d(s0) 

X5cg  =  x5  +  +<4d(  s0)  (2-20) 

X8cg  =  X8  '  ®d(c0) 

T 

Finally,  the  ownship  acceleration  vector  (a£x»  afy»  a£z’  is 
determined  from  ownship  accelerometers  which  are  located  near 
the  aircraft  c.g.  The  total  target  acceleration  (radar 
frame)  is  described  by  states  X^,  X^,  and  X^.  The  relative 
acceleration  can  be  found  by  simply  subtracting  total 
inertial  target  acceleration  from  ownship  acceleration. 


Thus,  the  acceleration  terms  at  the  radar  tracker  must  be 
transformed  to  the  inertial  frame.  This  is  accomplished  by 
the  following  formula  (by  the  Corolis  theorem): 


®x,y,z  =  —tracker  +  +  w*(wxd) 


(2-21) 


Expressing  the  last  two  terms  of  this  equation  in  inertial 
terms  and  evaluating  results  in: 


x3-ed(se)  -  ef(dce)  -  mu(dse)  +  mduen 
a  „  =  XA+flJd(c0)  +  2(ds0)  -  ffi(0(ds0)  -  tf(dc0)) 

C  •  g  «  D 

X9-ffl(dc0)  +  i(i(ds0)  +  ffi(dc0) )  +02(ds0) 


(2-22) 


2 . 5  Measurement  Models 
2.5.1  Introduction 

As  discussed  in  Section  2.2  and  the  last  section,  the 
F-4E/G  radar  is  located  in  the  nose  of  the  aircraft,  which 
rotates  about  the  aircraft  c.g.  during  a  maneuver.  Since  the 
distance,  d,  between  the  c.g.  and  the  radar  is  fixed,  it  is 
possible  to  account  for  both  the  translation  effects  from  the 
c.g.  to  the  radar  unit  and  rotational  effects  about  the  c.g. 
The  effects  do  not  impact  on  the  measurement  model  directly. 
The  measurement  model  is  referenced  to  the  tracker  frame  in 
order  to  compare  actual  measurements  directly  to 
"measurements"  from  the  measurement  model,  i.e.,  to  allow 
simple  residual  generation  in  the  eventual  filter. 


2.5.2  Measurement  Model  (Radar  Reference  Frame) 


Actual  noise-corrupted  measurement  realizations  are 
provided  by  the  radar  as  range  (R),  range  rate  (Rj^qj), 
azimuth  angle  (A-,),  elevation  angle  (E^),  azi-muth  rate  (w^  or 
AZDOT^’  anc*  el-evat*-on  rate  (w.  or  EtnAT).  The  Kalman  filter 


LDOT' 


requires  discrete-time  measurements  modeled  in  terms  of  the 
states  in  order  to  form  a  residual  (see  Section  2.6).  As 
discussed  in  Section  2.2,  the  measurement  equations  are 
nonlinear  when  modeled  in  a  Cartesian  frame.  Figures  2-7, 
2-8,  and  2-9  illustrate  the  geometry. 

The  following  are  measurement  equations  based  on  the 
state  space  and  the  geometry  defined. 

For  range,  R  (see  Figure  2-7): 


Zx  =  R  =  (X12  +  X42  +  X?2)l/2  +  vt 


(2-23) 


where  v^  is  described  statistically  in  Table  II. 4. 
For  range  rate,  RnnT: 


RDOT  “  R 

Xl(X2"vfx)  +  X4<X5-vfy>  +  X7(X8'vfz) 


+  X^  +  X7  ) 


+  v2  (2-24) 


where  v2  is  described  in  Table  II-4. 


V- 


For  azimuth  angle,  Az,  the  total  azimuth  angle  from  the 
geometry  (see  Figure  2-8)  is 


AZT  "  AZ  +  AZE 


(2-25) 


Slzt  =  tan"1(X4/X1) 


(2-26) 


where  AZE  is  the  error  angle  between  the  radar  boresight  and 
the  line-of-sight  (1)  vector  projection  onto  the  azimuth 
plane-  This  angle  is  typically  small  (8:3-361)  and  is 
modeled  as  zero  mean  white  noise.  Thus, 


Z^  =  Az  =  tan  X(X4/X^)  +  v^ 


(2-27) 


where  includes  noise  from  Table  II-4  plus  the  azimuth 


error . 


For  elevation  angle,  E^,  the  total  elevation  angle  from 
the  geometry  (see  Figure  2-9)  is 


ELT  '  EL  +  ELE 


(2-28) 


Elt  =  -tan 


(*1  +  V> 


(2-29) 


where  E^E  is  the  error  angle  between  the  radar  boresight  and 
the  1  vector  projection  onto  the  elevation  plane.  Again, 
this  angle  is  typically  small  and  is  modeled  as  zero  mean 
white  noise.  Thus, 


Z4  *  EL  -  -“"'l  (Xii  *  u4 


(2-30) 


where  v^  includes  noises  from  Table  II-4  plus  the  elevation 


error  term, 


For  azimuth  rate,  w^  or  ^DOT" 


wk  ~  AZD0T  '  AZ 


Xi(X5-vf, 


-  (Vvfx)X4 


where  is  described  in  Table  II-4. 

Finally,  for  elevation  rate,  Wj  or  ELDOT: 


(2-31) 


Z6  '  wj  =  ELD0T  =  EL 

>Vv£z)lXl2  *  X42)  -  X7tXl<X2-v£x»  *  X4(X3-''£y»! 


(X^  +  X^2  +  X?2)(X12  +  X42)1/2 


where  v^  is  described  in  Table  II-4. 


(2-32) 


For  Z^  and  Z^,  the  "azimuth  and  elevation  angle  errors 
can  be  as  large  as  0.5  degree"  (8:3-361).  However,  the  truth 
model  (see  Section  4-2)  outputs  significantly  larger  angle 
errors,  as  large  as  2.35  degrees  when  the  fighter  is 
maneuvering.  Concurrent  with  the  angle  errors,  azimuth  and 
elevation  angle  rate  errors  as  large  as  7.2  degrees  per 
second  are  observed.  00-ALC/MMECB  indicates  that  the 
observed  truth  model  errors  are  realistic  for  the  F-4E/G 
aircraft  (15).  The  large  angle  errors  and  angle  rate  errors 


.Vk*- 
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are  attributed  to  the  radar  dynamics.  Treating  2.35  degrees 
as  a  three-sigma  value  and  modeling  the  error  as  zero-mean 
white  Gaussian  noise  results  in  a  one-sigma  value  of  0.783 
degrees.  Adding  the  nominal  one-sigma  noise  value  from  Table 
II-4  results  in  an  overall  one-sigma  value  of  approximately 
one  degree.  Similarily,  the  angle  rate  error  terms  observed 
plus  the  nominal  value  results  in  a  one-sigma  value  of 
approximately  3.1  degrees  per  second.  From  simulation  runs 
conducted  in  Chapter  V,  a  one-sigma  of  3.1  is  determined  to 
be  too  large  (by  studying  measurement  residual  plots)  and 
consequently  reduced  to  1.26  degrees  per  second. 

Additionally,  the  range  and  range  rate  terms  of  Rnom  are 
considered  too  small  and  are  increased  as  shown  in  Equation 
(2-33). 
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(2-33) 


0 


0 


0 


0 


0 


Summarizing  Che  measurement  equations  yields 


z( t^ )  =  h[x( ) ]  +  v 


V  *  *  X7V'2 

V1 

xl<x2-vfx)  ♦  X4(X5-v£v)  ♦  X7(X8-v£2) 

(X12  +  X42  +  x7V/* 

v2 

tan"1(X4/X1) 

>3 

! 

_  -1  X7 

-tan  - « - — TTTT~ 

(X12  +  x4^)1/z 

+ 

v4 

Xl>X5-vfv)  -  <X2-v£x>x4 

j 

t 

i 

X  2  +  X  2 

XI  X4 

5 

(X8-vfZ)(Xl2  -  X42)  -  X7lx1(X2-v£x)  *  X4(X5-v£v)] 

(Xj2  +  X42  +  X72)(X12  +  x42)1/2 

- 

v6 

(2 

-34) 

2 . 6  Truth  Model 

The  truth  model  is  comprised  of  the  radar  servo  model 
and  associated  true  target  and  filter  states  derived  from  a 
trajectory  generation  program  (see  Chapter  IV).  For  the 
purpose  of  evaluating  the  filter  performance,  it  is  only 
necessary  to  generate  data  points  of  relative  positions, 
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total  target  velocities,  and  total  target  accelerations  at 


discrete  times,  t^.  For  the  purpose  of  computer  simulation, 
it  is  not  necessary  for  the  fighter  to  follow  the  target, 
only  to  track  it.  Thus  the  information  from  the  Kalman 
filter  is  not  fed  back  to  the  simulated  aircraft  (as  it  will 
be  to  an  actual  aircraft  to  determine  control  inputs), 
simulated  filter  outputs  are  only  compared  to  the  true  states 
to  generate  a  statistical  analysis.  This  is  presented 
pictorially  in  Figure  2-10. 


i - 1 


Figure  2-10  Performance  Evaluation  of  the  Kalman  Filter 


III.  EXTENDED  KALMAN  FILTER  DESIGN 


3 . 1  Introduction 

The  purpose  of  this  chapter  is  to  derive  appropriate 
models  for  various  Kalman  filters  for  theoretical  study  and 
possible  implementation  on  the  F-4E/G.  It  is  shown  in 
Chapter  II  that  the  selection  of  a  Cartesian  coordinate  frame 
resulted  in  nonlinear  measurement  equations.  Thus  a  linear 
Kalman  filter  implementation  can  not  be  employed.  A 
linearized,  or  perturbation  Kalman  filter  can  be  used  but  the 
nominal  trajectory  for  an  air-to-air  tracking  scenario  is  not 
known  a  priori.  Choosing  an  arbitrary  nominal  path  results 
in  large  perturbations  and  can  cause  filter  divergence. 
Moreover,  an  extended  Kalman  filter  ( EKF )  has  the  same  basic 
form  as  the  linear  and  linearized  Kalman  filter  except  that 
h [ t i  ),t is  used  to  form  the  residual,  rather  than 
(h[x  ,  t,  ]  +  H  x).  An  EKF  uses  the  state  estimates  to 
relinearize  the  filter  about  a  new  reference  state  trajectory 
each  time  a  new  state  estimate  vector  is  calculated.  The 
linearization  is  a  first  order  approximation  of  a  Taylor 
series  expansion  about  the  estimate  of  the  state  vector. 
Higher  order  filters  such  as  modified  truncated  second  order 
filters  and  modified  Gaussian  second  order  filters  provide 
performance  enhancement  over  the  EKF  by  reducing  estimate 
bias  but  with  added  computational  complexity  (16:221-223). 
Because  of  the  limited  memory  of  the  F-4E/G  fire  control 
system,  the  filter  employed  in  this  research  is  limited  to  a 


first  order  filter  without  bias  correction.  As  it  is  shown 
in  Chapter  V,  bias  correction  is  not  necessary  because  the 
system  displays  essentially  zero-mean  performance  when  the 
fighter  is  not  maneuvering. 

As  explained  in  Section  1.2,  the  present  F-4E/G  target 
estimation  filter  does  not  accurately  estimate  target 
parameters  during  an  F-4  maneuver.  It  may  be  the  current 
Wiener-Hopf  filter  is  improperly  tuned.  An  EKF  that  is 
properly  tuned  may  in  itself  provide  adequate  target 
estimates.  Alternately,  an  off-line  adaptive  EKF  is  studied 
for  its  effect  on  response.  An  adaptive  filter  varies  filter 
parameters  (as  gain  and/or  Q  and  R)  in  response  to  a  given 
decision  used  to  change  the  weighting  of  measurement  for 
incorrect  system  modeling.  "Off-line"  adaptive  estimation  is 
a  process  of  varying  the  filter  tuning  parameters,  and  thus 
gain,  to  alter  filter  performance  based  on  a  priori 
information.  Off-line  adaptive  estimation  provides  a 
baseline  of  performance  that  an  on-line  adaptive  estimator 
can  theoretically  approach.  Since  the  acceleration  of  the 
target  is  random  with  respect  to  the  fighter,  it  may  be 
necessary  to  make  adaptive  adjustments  during  periods  of 
detected  acceleration  to  correct  for  changed  target  behavior. 
Furthermore,  as  it  is  shown  later,  filter  performance  is  the 
worst  when  the  fighter  maneuvers,  and  therefore,  it  probably 
is  necessary  to  make  adaptive  changes  during  a  fighter 
maneuver . 

There  has  been  considerable  research  done  on  using  a 


variation  of  a  Kalman  filter  for  solving  precision  pointing 
and  tracking  problem.  Appendix  A  contains  abstracts  from 
selected  theses  and  published  articles  which  studied  Kalman 
filters.  Thus  the  problem  has  been  studied  before  and  the 
application  of  the  Kalman  filter  is  well  established. 

3.2  EKF  Design 

In  this  section,  EKF  equations  are  presented  without 
derivation.  Then  the  required  f  and  h  vectors  and 
corresponding  F  and  H  partial  derivative  matrices  for  use  in 
the  EKF  design  are  derived.  Next,  Q  is  calculated  based  on  a 
X  ,7  ,  for  a  fighter  type  aircraft.  This  is  followed  by 
evaluation  of  initial  conditions,  x^  and  P^.  These  steps 
constitute  the  design  and  allow  for  a  follow-on  system 
evaluation . 

3.2.1  EKF  Equations 

Using  page  44  of  Reference  16  as  a  guide,  the  EKF 
equations  are  presented  without  proof.  From  Equations  (2-10) 
and  (2-11)  the  dynamics  model  of  interest  is 

x(t)  =  f[x(t),u(t),t]  +  G ( t )w( t )  (3-1) 

where : 

x(t)  is  the  n-state  filter  vector  [  x(t  )  is 

—  —  o 

modeled  as  a  Gaussian  random  vector  with  mean  x 

— o 

and  covariance  P  1 
— o 

£[x(t) ,u(t) ,t]  is  the  filter  dynamics  vector 


u(t)  is  a  r-vector  of  known  input  functions 


G(t)  is  a  n-by-s  noise  input  matrix,  and 

w(t)  is  a  zero-mean  white  Gaussian  s-vector  process 
with  strength  Q(t), 

E[w(t)wT(t+T)  ]  =  Q(t)(5(T)  (3-2) 

and  independent  of  x(t  ). 

From  Equation  (2-34),  the  available  discrete-time 
measurements  are  modeled  as  the  m-vector  process  y(t^) 

z(ti)  =  h[x(ti),ti]  +  v(t£)  (3-3) 

where , 

y(tt)  is  a  zero-mean  white  Gaussian  m-vector 
process  with  covariance  RU.^,  independent  of  x(tQ) 
and  w( t ) , 

h[*(tj_)  ,tjJ  is  the  measurement  model  vector. 

The  measurement  update  incorporates  measurements 
z(ti}Wj)  =  by 

K(t1)-[P(ti")HT[ti;£(ti")}] 

[H[ti;^(ti")]P(ti")HT[ti;£(ti-)]  +  R(t1 ) ]-1  (3-4) 

x(ti  +  )  =  x ( t )  +  KU^Hz^  -  hfxf^”),^]]  (3-5) 

P(tL+)  =  P(ti“)  -  K(ti)H[ti;x(tl")]P(ti')  (3-6) 

where , 
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K ( t ^ )  is  the  Kalman  filter  gain, 


H[t^;x(t^  )]  is  the  m-by-n  partial  derivative 
matrix 


H[ti;x(ti  )]  = 


B  * 


x=x(t.  ) 


,and 


(3-7) 


P(t^)  is  the  filter-computed  conditional  covariance 
matrix;  "+"  specifies  covariance  is  referenced  to 
time  just  after  update  time  t^,  specifies  just 

before  time  t^. 

The  estimate  is  propagated  forward  to  the  next  sample  time 
Ci+1  ^  integrating 

x(t/ti)  =  f[x(t/ti),u(t),t]  (3-8) 

P(t/t.)  =  F[t;£(t/ti)]P(t/ti)  +  P(t/ti)FT[t;x(t/ti)] 

+  G(t)Q(t)GT(t)  (3-9) 


from  time  t^  to  ti  +  1,  where  (t/t^)  represents  time  t  for  t  as 
an  element  of  [tiSt^+^)  (given  measurements  through  time  t^). 
Additionally,  the  initial  conditions  are  provided  in 
Equations  (3-5)  and  (3-6)  as 

E(ti/ti)  =  R(ti+)  (3-10) 


P(t1/t1)  =  P(tt+)  (3-11) 

For  the  first  interval,  time  tQ  to  tj_,  the  initial  conditions 
are  x^  and  PQ.  In  Equation  (3-9),  F[tj£(t/t^)]  is  the  n-by-n 

49 


partial  derivative  matrix: 


F[t;x(t/t.)]  = 


3  f  [  >i>H(  c  )  >  t  ] 


x=x ( t/t L ) 


(3-12; 


for  all  t  in  the  interval  [ti,ti+1).  As  discussed  in  Section 
2.2,  using  a  Cartesian  coordinate  frame  for  the  dynamics 
models  of  Chapter  2  results  in  £(=Fx)  being  linear  and  time- 


invariant  . 


Finally,  by  integrating  Equations  (3-8)  and  (3-9)  to 
the  next  sample  time,  x(ti+1“)  and  £(ti+^_)  are  defined  as 


*<ti+i  >  =  £(t1+1/tt) 


(3-13) 


i^i  +  P  =  P(ti  +  l/ti) 


(3-14) 


for  use  in  the  next  measurement  update. 

The  theory  presented  in  this  section  is  applied  in  a 
computer-aided  design  package  called  SOFE  (see  Section  4-3). 
However,  as  explained  in  Section  2.4.3,  a  state  transition 
matrix  approach  can  be  used  to  simplify  the  above  propagation 
( see  Section  4.4) . 

3.2.2  Evaluation  of  H[ t^ ;x( t^~ ) ]  and  F[t;x(t/t.)] 


For  the  dynamics  measurement  model  in  Chapter  II, 
Equation  (2-34),  H[t^;x(t^  )]  is  derived.  Next,  for  the 
state  vector  differential  equation,  Equation  (2-10), 
F[t;x(t/t^)]  is  calculated.  The  results  are  used  in  the  EKF 
design. 

For  Htt^jxft^)  ] , 


v- 


H[ti;x(ti“) ] 


where , 


Ji 

41 

0 

0 

H44 

0 

0 

H47 

0 

0 

(3 

51 

H52 

0 

H54 

H55 

0 

0 

0 

0 

61 

H62 

0 

H64 

H65 

0 

H67 

H68 

0 

(xl  +x4  +x7  ) 


H17  = 


(x^+x^+x/)*'4 
(x2'vfx} (x12+x42+x72)" 


H2i=- 


xl[xl( x2'vf x > +x4 ( x5-vfy } +x7 ( x8"vf z  > ] 


<X1  +x4  +x7  > 


(3-16) 


(3-17) 


(3-18) 


(3-19) 


( +X7  ) 


(3-20) 


(x^~V£y)(X^  +X4  +X7  )■ 


H24= 


x4[ xl ( x2~vf x ) +x4( x5'vf y  ]  +x7 ( x8'vf z > ] 


(X1  't‘x4  +x7  ) 


(3-21) 


(3-22) 


(3-33) 


xf+x4 


-[(x^  +x4  +x7  )(x^  +  x4  )( 2x^ (Xg-Vg z ^ ~ ^x2~ x ^*7 ^ ” 

2  2 

[(x8"vfz)(xl  +x4  )-x7(xi(x2_vfx)+x4(x5-vfy) J ^xl^ 


H61  = 


[2(x1z+x4'c)  +  (x1/  +  x4z  +  x7z)  ]  ] 

(x12+x42+x72)2(x12+x4: 


(3-34) 


H62~ 


xlx7 

(xx2+x42+x72 ) (x^  +  x^ ) 


(3-35) 


[ (x^  +x4  +x 7  )(x^  +x4  ) ( 2x4 ( xg~V£z )  (x^  Vf y ) x 7 )  — 

2  2 

[(x8-vf2)(xi  +x4  )-x7(x1(x2-vfx)+x4(x5-vf  ))][x4] 


H64= 


[2(x12+x42)+(x12+x42+x72) ] ] 

(x1ii+x4i+x72)2(x12+x42): 


(3-36) 


H65= 


(x^+x^+x^ )  (x^+x^ ) 


(3-37) 


H67=- 


(x^  +x4  +x7  ) (x1  +x4  ) ( x^ ( x2~^f x ^ +x4 ^ x5_vfy ^ + 

o  2 

^(x8”vfz)(xl  +x4  )-x7(x1(x2-vfx)+x4(x5~vfy) > 3* 
[ 2 ] [ x7 ] [ (x12+x42)] 


(x12  +  x42  +  x72 )2(x^2+x42  )3/2 


(3-38) 


H68= 


-<x!2+x42) 

(x^2+x42+x72 ) (x-^  +  x^ ) 


(3-39) 


F[t;x(t/t^)]  is  identical  to  F  derived  in  Equations 


(2-10)  and  (2-11). 


in. 


3.2.3  EKF  Noise  Strengths 


The  measurement  noises  are  derived  in  Chapter  II  and 

are  summarized  in  Equation  (2-34).  The  strengths  of  the 

dynamic  driving  noises  wx,  w^,  and  wz  (see  Equations  (2.9) 

and  (2.10))  represent  the  uncertainties  in  the  choices  of  T  . 

x  ’ 

%>  and  for  the  acceleration  state  equations.  In  matrix 

y  £ 

form,  Equation  C’-2)  becomes 


Q(t ) 


Qj_  0  0 

0  q2  0 

0  0  q3 


(3-40) 


The  initial  choice  of  values  for  ,  Q2,  and  Q3  are  not 
critical  since  Q  values  are  changed  during  the  filter  tuning 
process  to  obtain  the  best  filter  performance  (see  Chapter 
V).  On  the  other  hand,  since  R  is  based  on  the  available 
measurements,  R  is  not  normally  varied  in  this  study.  Thus, 
the  initial  value  of  Q  is  selected  as  follows 

<31,2,3  '  2o2^x,y,*  <3-41) 

Using  a  T equal  to  0.5  seconds,  which  is  typical  for  a 
fighter  type  aircraft,  a  cr  equal  to  3.0  g's  or  96.6  ft/sec2 
(17),  and  assuming  symmetry  in  all  axes  (not  unrealistic 
because  target  acceleration  is  random  to  the  fighter  and  is 
as  probable  to  occur  in  one  axis  as  another)  results  in 
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37,325  0 


Q(t)  =  0 


37,325  0 


(3-42) 


37,325 


2  5 

where  the  units  of  Q  are  feet  /seconds  . 


3.2.4  Initial  Conditions,  >?  and  P 

— o  — o 


As  discussed  in  Section  3.2,  x(t  )  is  a  Gaussian  random 

—  o 

variable  with  mean  and  covariance  Conceptially , 

can  be  obtained  from  the  first  measurement.  (For  simulation, 
these  are  the  states  at  the  start  of  the  simulation;  for 
actual  implementation,  the  first  measurement  after  radar 
lock-on. )  The  covariance  P^  represents  the  Gaussian 
distribution  about  x^,  or  the  initial  state  uncertainty. 

Since  P„  is  not  known  a  priori,  an  initial  value  must  be 
assumed  based  on  model  parameters.  Unfortunately,  model 
parameters  vary  significantly  for  different  types  of 
engagements  (i.e.,  a  beam  on  or  tail  chase  attack). 

Therefore,  is  eventually  selected,  based  on  a  sensitivity 
study  (see  Chapter  V),  such  that  the  initial  covariance  is 
high.  Then,  through  several  filter  propagation  and  update 
cycles  the  covariance  converges  to  a  reasonable  value  using 
the  actual  measurement  history.  In  order  to  initially  test  a 
filter,  a  value  of  P^  must  be  assumed.  Assuming  symmetry  in 
all  axes  (again  not  unreasonable),  P  is  selected  based  on 

— O 

root-mean-square  values  of  25  feet  for  position,  200 
feet/second  for  velocity,  and  three  g's  (96.6  feet/second) 


for  acceleration  (17).  Thus, 

1000  0  0000000 

0  4000  0  0  0  0  0  0  0 

0  0  9300  0  0  0  0  0  0 

0  0  0  1000  0  0  0  0  0 
=  0  0  0  0  4000  0  0  0  0  (3-43) 

0  0  0  0  0  9300  0  0  0 

0  0  0  0  0  0  1000  0  0 

0  0  0  0  0  0  0  4000  0 

0  0  000000  9300 


3 . 3  Equivalent  Discrete-Time  EKF  Design 

The  design  presented  in  Section  3.2  is  based  on  a 
continuous  time  dynamics  model  and  is  useful  for  evaluating 
filter  performance.  But  for  actual  implementation,  a 
continuous  time  dynamics  model  is  not  feasible  because  of  the 
F-4E/G  on-board  digital  computer.  This  motivates  the  need 
for  an  equivalent  discrete-time  system  model.  The 
discrete-time  update  relations  remain  identical  to  those 
presented  in  Section  3.2.  Additionally,  the  equivalent 
discrete-time  propagation  relations  can  be  reduced  in 
complexity  to  that  of  a  linear  Kalman  filter.  These  two 
points  significantly  reduce  design  complexity,  which  in  turn 
reduces  the  memory  storage  and  processing  time  required  for  a 
digital  simulation  or  implementation. 


3-3.1  Equivalent  Discrete-Time  System  Model  Design 


As  previously  discussed,  the  measurements  are  only 
available  at  discrete-time  intervals.  Since  the  update 
relations  of  Section  3.2,  Equations  (3-5),  (3-6),  and  (3-7), 
are  based  on  discrete-time  measurements,  the  update  relations 
remain  unchanged.  The  equivalent  discrete-time  propagation 
equations  analogous  to  the  continuous  time  equations  of 
Section  3.2,  Equations  (3-8)  and  (3-9),  are  presented  without 
derivation,  using  pages  170-172  of  Reference  5  and  pages 
45-46  of  Reference  16  as  a  guide.  For  the  general  case,  the 
time  propagations  can  be  written  equivalently  as 

x(t1+1“)  =  X(t.+)  +  rti+1  f[£(t/t.),u(t),t]dt  (3-44) 

J  Ci 

P(ti+i")  =  ®[ti+1,ti;£(T/ti)]P(ti+)fflT[t.+1,ti;£(T/ti)] 

+  {  Z 1 +1®  C  t  + 1 ,  t ;  x  ( T/t .  )  ] G  ( t  )$(  t  )GT ( t ) fflT[  t  ,  t ; S  (77 1  - )  ] dt 

J tt  i  i  -  t3-45) 

Since  F  is  linear  and  time-invariant,  the  stochastic 
difference  equation  reduces  to  standard  linear  propagation 

x(ti+1)  -  ffl(tl+1,ti)x(ti)+Bd(ti)u(ti)+wd(t.)  (3-46) 

where, 

— (ti  +  i>ti)  Is  the  state  transition  matrix  and  is 
derived  in  Equations  (2-12)  through  (2-14), 


t i )  is  the  discrete-time  input  matrix  defined  by 


Bd(ti).  J  *0(ti+1,T)B(TidT, 


( 3-46a ) 


(assuming  u(Ci)  is  constant  on  t^=t=t£+^)  an<^ 

w,(t.)  is  an  s-vector-valued  white  Gaussian 
—a  1 

discrete-time  stochastic  process  with  mean  zero  and 
covariance  kernel 

-  /  gd<tt)  tt-C. 

I-  iT  j 

Qd(t.)=  Jti  +  1  ffl[t.+lsT]G(T)2(T)GT(T)a)T[ti+1>T]dr 
1  ( 3-46b ) 


The  time  propagation  relation  for  Equations  (3-44)  and 
(3-45)  reduce  to  (5:275) 


£(ti+1-)  =  ffl(t.+1,ti)^(ti+)+Bd(ti)u(ti) 


(3-47) 


+  T 


P(ti+i  )  =  ffl(ti+1>ti)P(ti  )®1(t.+1,ti)  +  Qd(ti)  (3-48) 

3.3.2  Evaluation  of  Equivalent  Discrete-Time  Variables 

To  complete  the  design  of  the  equivalent  discrete-time 

model,  (D(t^  +  ^,t^),  Xq,  E^,  B^,  and  Q  ^  must  be  derived. 

<D(ti+l,ti)  is  derived  in  Equations  (2-11)  through  (2-14). 

Initial  conditions  x  and  P  remain  as  presented  in  Section 

— o  — o 

3.2.4. 

For  B^,  evaluating  Equation  (3-46a)  results  in 
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At  0  0 

0  0 


0  0 
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0 

0  0 


0  0 
-At  0 
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0  0  -At 


0  0 


0  0 


Evaluating  Equation  (3-46b)  results  in 
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0 
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0 
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0 

Q31 
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0 

0 

0 

0 

0 

0 
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0 

Q44 

Q45 

Q46 

0 

0 

0 

0 

0 

0 
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Q55 

Q56 

0 

0 

0 

0 

0 

0 

V 
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^66 

0 

0 

0 

0 

0 

0 

0 

0 

0 

Q77 
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0 

0 

0 

0 

0 

0 

Q87 
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0 

0 

0 

0 

0 

0 

Q97 

Q98 

Q 
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where,  for  the  first  block  of  the  diagonal, 
Qi1=[QiTi5/2][i-e-(2At/Ti)^At/r^At3/3ri^2At2/ri^ 


(4/Ti)Ate"(At//,7i)  ] , 


Q12  =  [QlTi4/2  ]  [  1  +  e" (  2At/Ti  5  "2e‘  (  At /Ti  ’  +  (  2/ T.  )  Ate"  ( At  /Ti  1 
f2At/T^At2/T.2], 

Ql3=CQiri3/2][l"e"(2At/ri)‘(2/Ti)At:e"(At:/7i)  1* 


Q21=Q12, 


Q22=[QlTi3/2][4e"(At/Ti)"3'e”(2At:/Ti)+2At:/Ti  ] 

Q23=[QiTi2/2][1+e‘(2At:/ri)-2e'(At:/ri)  ], 


Q31~Q13, 


Q32=Q23, 


Q33=[QlTi/2][1_e"(2At/7i)] 


At=.04  seconds  (initially), 


71=0.5  seconds  (initially),  and 


Q1=Q2=Q3=37 , 325  initially  (before  tuning) 


The  second  and  third  blocks  of  the  diagonal  are  the  same  as 

the  first  but  with  the  subscripts  of  Q  changed  and  Q1 

xy  i 

changed  to  either  Q2  or  Q^* 


IV.  METHODS  OF  MODEL  SIMULATION  AND  TESTING 


4.1  Introduction 

The  continuous  time  models  contained  in  Chapter  II  and 
the  Kalman  filter  equations  in  Chapter  III  are  developed  for 
simulation  and  testing  utilizing  a  well  established  computer- 
aided  design  package  called  Simulation  for  Optimal  Filter 
Evaluation  (SOFE) (18).  The  equivalent  discrete-time  dynamics 
model  and  Kalman  filter  equations  in  Chapter  III  are 
developed  for  simulation  on  a  digital  computer  as  a 
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stand-alone  program  for  possible  implementation  on  the  F-4E/G 
The  stand-alone  simulation  (SAS)  program  developed  for  this 
thesis  is  fully  functional  and  provides  performance  similar 
to  that  of  the  SOFE  simulation.  The  advantage  of  the  SAS 
program  over  SOFE  is  that  it  is  an  equivalent  discrete-time 
design  containing  the  specific  target  estimation  filter 
designed  in  Chapters  II  and  III.  Additionally,  since  it  is 
an  equivalent  discrete-time  design,  it  can  easily  be 


"N'Nv 


implemented  on  the  F-4E/G  digital  computer.  As  such,  the  SAS 
program  is  recommended  for  further  testing  and  possible 
implementation  on  the  F-4E/G  testing  facilities  or  aircraft. 


of  this  chapter  is  to  describe  the  truth  model  trajectory 
generation,  SOFE  implementation  and  testing  procedures,  SAS 
implementation  and  testing  procedures,  and  the  filter  tuning 
philosophy  employed. 

4.2  Tra iectorv  Generation  (Truth  Model) 


An  F-4E/G  trajectory  generation  simulation  model  which 
provides  the  basic  aircraft  and  target  dynamics,  problem 
geometry,  radar  model  with  antenna  dynamics,  and 
truth/measurement  data  required  to  perform  a  Monte  Carlo 
analysis,  was  obtained  from  OO-ALC/MMECB  (12).  As  discussed 
below,  the  trajectory  simulation  coding  and  output  format  is 
slightly  modified  in  this  thesis.  The  changes  are  necessary 
for  computer  system  compatiblity  and  coordinate  frame 
rotations  into  the  filter  frame.  The  computer  simulation 
source  code  is  included  in  Appendix  B.  The  changes  made  are 
as  follows: 


1.  The  program  received  from  OO-ALC  included  some 
non-standard  (VAX  VMS-specific)  Fortran  code.  The 
non-standard  coding  is  replaced  by  Fortran  5  coding  so 
a  standard  Fortran  5  compiler  can  be  used.  In 
particular,  for  this  effort,  the  code  is  compiled  on  a 
CDC  CYBER  compatible  compiler  which  does  not  recognize 
the  VAX  VMS-specific  code.  The  non-standard  code  (see 
Appendix  B)  is  left  in  the  source  code  as  a  comment 
line  and  the  appropriate  Fortran  5  statement  entered 
after  the  comment  line. 


acceleration  to  target  accelerations  in  the  target  body 
axes.  This  is  necessary  to  obtain  the  three- 
dimensional  acceleration  for  use  in  the  performance 
evaluation  comparison  of  the  acceleration  truth  states 
and  filter  states. 

3.  All  the  data  required  for  SOFE  or  the  SAS  program, 
is  rotated  into  one  reference  frame,  the  antenna 
reference  frame  ( i-0 >  j0  jkQ ) .  Filter  states  are  also 
expressed  in  this  frame,  which  allows  direct  comparison 
between  the  filter  states  and  truth  states. 

4.  Finally,  by  describing  the  filter  states  in  a  body 
reference  frame  (the  antenna  reference  frame),  the 
truth  trajectory  generation  model  is  transformed  from  a 
two-dimensional  simulation  to  a  three-dimensional 
simulation.  The  trajectory  generation  program  provided 
by  OO-ALC  is  a  two-dimensional  simulation.  The 
position,  velocity,  and  acceleration  states 
corresponding  to  the  down  direction  from  the  horizontal 
plane  are  zero  throughout  the  simulation.  In  order  to 
obtain  peformance  evaluation  in  the  down  direction,  the 
truth  model  is  transformed  into  a  three-dimensional 
simulation  by  an  appropriate  choice  of  fighter 
coordinate  axes  (thus  avoiding  a  major  truth  model 
modification).  The  transformation  occurs  whenever  the 
fighter  rolls  in  the  horizontal  simulation  plane.  The 
states  roll  with  the  fighter,  resulting  in  nonzero 


values  for  states  even  though  the  simulation  trajectory 
stays  in  the  horizontal  plane.  In  this  manner,  all 
modes  and  states  of  the  three-dimensional  problem  are 
excited  and  therefore  can  be  analyzed  in  the 
performance  analysis. 


The  last  three  modifications  described  above  are  added 
to  the  basic  trajectory  generation  as  an  user-selected 
option.  Overall,  the  basic  truth  model  received  from  OO-ALC 
is  left  intact.  In  fact,  OO-ALC  provided  a  sample  trajectory 
output  listing  from  the  basic  program.  After  all 
modifications  were  completed,  the  basic  program  option  was 
rerun  and  the  output  trajectory  found  to  be  identical  to  that 
provided  bv  OO-ALC. 

Using  the  modified  trajectory  generation  program  with 
the  coordinate  change  option,  two  test  trajectories  are 
generated  for  Kalman  filter  testing.  The  trajectories  used 
are  a  beam  shot  attack  and  a  tail  chase  attack,  both  of  which 
are  described  in  detail  below. 

4.2.1  Trajectory  Generation  -  Beam  Attack 

A  beam  attack  trajectory  simulation  is  selected  for 
analysis  since  it  is  often  employed  in  an  air-to-air 
engagement.  Additionally,  the  beam  attack  trajectory 
involves  numerous  fighter  maneuvers  which  are  of  direct 
interest  to  this  study  (since  the  current  Wiener-Hopf  filter 
becomes  unstable  when  the  fighter  maneuvers).  The  beam 
trajectory  progresses  as  follows.  Initially,  the  fighter  and 


target  are  at  a  range  of  40,000  feet,  at  the  same  altitude 
(arbitrary  but  below  32,000  feet,  by  definition  from  Section 
1.4),  both  with  an  airspeed  of  800  feet/second,  and  are 
flying  at  right  angles  to  each  other  with  the  target  45 
degrees  to  the  right  of  the  centerline  extending  from  the 
fighter's  nose  (see  Figure  4-1).  One  second  after  the 
simulation  starts,  the  target  starts  a  three-g  level  turn  to 
its  right.  At  three  seconds,  the  fighter  rolls  right  into 
the  direction  of  the  target  and  at  after  four  seconds  has 
established  a  two-g  turn  (approximately  a  60-degree  bank 
angle).  This  turn  is  maintained  from  simulation  time  of  four 
to  five  seconds.  At  five  seconds,  the  fighter  starts  a 
reverse  roll  to  the  left  and  rolls  out  essentially 
wings-level  shortly  after  six  seconds.  A  constant  fighter 
heading  is  maintained  for  the  rest  of  the  simulation.  At  9 
seconds,  the  target  starts  a  left  roll  and  attains 
essentially  straight-and-level  flight  at  about  10  seconds. 

The  simulation  is  terminated  at  12  seconds. 

4.2.2  Trajectory  Simulation  -  Tall  Chase 

The  tail  chase  trajectory  simulation  is  selected  for 
analysis  for  the  same  reasons  as  the  beam  attack  trajectory. 
The  tail  chase  trajectory  progresses  as  follows  (see  Figure 
4-2).  Initially,  the  fighter  and  target  are  at  a  range  of 
10,000  feet  with  the  same  heading  (arbitrary),  same  altitude 
(arbitrary  but  below  32,000  feet),  and  at  the  same  airspeed 
(800  feet/second).  The  target  is  five  degrees  left  of  the 
fighter's  nose.  At  one  second,  the  target  starts  a 


three-g  level  turn  to  the  right,  turning  in  front  of  the 
fighter.  At  5  seconds,  the  fighter  rolls  right  in  pursuit  of 
the  target  and  then  (at  about  6  seconds)  maintains  a  2-g  turn 
(approximately  a  60-degree  bank)  which  is  held  for  the 
remainder  of  the  simulation.  The  target  crosses  in  front  of 
the  fighter's  nose  at  about  5.5  seconds.  Then,  at  about  6.6 
seconds  the  fighter's  nose  catches  up  to  the  target  and  the 
target  is  once  again  slightly  to  the  left  of  the  fighter's 
nose.  The  fighter  "tracks"  the  target  for  the  remainder  of 
the  simulation  with  the  target  staying  within  one  degree  of 
the  fighter's  nose. 

4.2.3  Trajectory  Simulation  Run  Time  Selection 


The  trajectory  simulations  are  run  at  0.02  second 
intervals  and  the  results  are  stored  in  an  external  data 
file.  SOFE  input  routines  read  and  interpret  trajectory  and 
measurement  data  from  the  stored  data  file,  while  the  SAS 
program  must  have  an  update  time  that  is  an  even  multiple  of, 
or  equal  to,  the  trajectory  simulation  time.  Since  some  of 
the  simulation  completed  in  this  effort  is  run  at  a  0.04 
second  update  period  (as  explained  in  Section  4.3.2),  0.02  is 
selected  for  the  trajectory  generation  simulation  time  to 
allow  points  on  either  side  of  the  SOFE  interpretation.  This 
in  turn,  allows  a  SAS  update  period  of  0.1  seconds. 


4.3  SOFE  Simulation  and  Testin, 


System  Validation 


4.3.1  Introduction 

The  purpose  of  the  SOFE  simulation  is  to  evaluate  how 
the  sampled-data  continuous-time  Kalman  filter  performs 
compared  to  the  truth  model  (trajectory  generation  of  last 
section).  SOFE  and  a  SOFE  Plotting  (SOFEPL)  (19)  program  are 
used  as  basic  tools  for  generating  the  data  required  for 
filter  evaluation.  These  programs  were  developed  by  the  Air 
Force  Avionics  Laboratory  as  general-purpose  programs  to  help 
design  and  evaluate  Kalman  filters  for  integrated  systems. 
SOFE  provides  the  basic  functions  required  to  perform  a  Monte 
Carlo  analysis  on  the  extended  Kalman  filter  designed  in 
earlier  chapters.  SOFE  contains  31  routines  to  perform  such 
tasks  as  input/output ,  problem  and  run  setup,  numerical 
solution  to  ordinary  differential  equations,  measurement 
update  through  a  Carlson  square  root  algorithm  (5:385),  and 
run  and  problem  termination  ( 18 : Abstract ) .  Nine  user-written 
subroutines  define  the  specific  system  and  extended  Kalman 
filter  under  study.  The  user-written  routines  used  in  this 
thesis  are  included  in  Appendix  C.  Equations  (2-9),  (2-10), 
(2-33),  (2-34),  (3-15)  through  (3-39),  (3-42),  (3-43),  and 
(3-50)  are  incorporated  into  the  user  subroutines  to  specify 
an  integrated  Kalman  filter  design.  The  outputs  of  SOFE  are 
stored  for  postprocessing  by  SOFEPL.  SOFE  outputs  records 
for  each  time  increment  containing  time,  the  truth  states, 
the  diagonal  elements  of  the  filter  computed  covariance,  the 
measurement  residuals,  and  the  residual  variances.  The 


output  records  are  postprocessed  by  SOFEPL  to  form  an 
ensemble  of  Monte  Carlo  runs  and  DISSPLA  (20)  to  produce 
graphical  Calcomp  (21)  plots  of  the  Monte  Carlo  analysis. 


Figure  4-3  illustrates  a  sample  Calcomp  plot  generated 
during  this  research.  The  plot  first  displays  the  mean  of 
the  error  states  (state  estimate  minus  the  truth  state). 
Second,  the  envelope  formed  by  the  mean  error  plus  or  minus 
the  standard  deviation  of  the  actual  error  is  illustrated. 
Finally,  the  envelope  formed  by  zero  plus  or  minus  the  square 
root  of  the  sampled-averaged  filter-computed  covariance 
diagonal  terms  is  displayed. 

4.3.2  Determining  Filter  Performance 

The  plots  generated  from  SOFE/SOFEPL/DISSPLA/Calcomp 
(SSDC)  processing  are  used  to  determine  how  a  particular 
Kalman  filter  performs.  Performance  is  determined  by 
comparing  different  plots  for  particular  tuning  factors. 
Ideally,  the  true  mean  error  plus  or  minus  the  one-sigma  time 
histories  should  stay  inside  the  envelope  formed  by  zero  plus 
or  minus  the  square  root  of  the  corresponding  filter-computed 
covariance  diagonal  terms  (see  Figure  4-3).  For  a  properly 
tuned  filter,  shortly  after  a  target  maneuver,  the  mean  error 
data  may  stray  outside  the  filter-computed  standard  deviation 
envelope  but  should  return.  Plots  from  various  simulation 
runs  having  different  tuning  values  of  R  (see  Equation 
(2-34),  ^  (see  Equation  (3-42),  and  tau  (see  Equations  (2-9) 
and  (2-10))  are  compared  to  determine  which  set  of  tuning 


factors  performs  the  best,  i.e.,  which  yields  the  least  mean 
error  and  results  in  the  true  mean  plus  or  minus  the  standard 
deviation  staying  reasonably  within  the  square  root  of  the 
filter-computed  covariance  diagonal  elements. 

Figure  4-4  presents  one  plot  of  sample  statistics 
computed  from  five  runs  of  the  overall  problem.  A  velocity 
state  mean  error  plot  is  chosen  since  the  velocity  states 
consistently  demonstrate  the  most  sensitivity  to  different 
tuning  factors  and  update  periods.  For  this  particular 
problem,  five  runs  are  sufficient  for  evaluation  between 
different  tuning  factors.  This  can  most  easily  be 
demonstrated  by  comparing  Figures  4-4  and  4-5.  Figure  4-4  is 
a  five-run  analysis  and  Figure  4-5  is  a  20-run  analysis,  both 
with  an  update  time  of  0.04  seconds.  Note  that  both  figures 
display  the  same  general  shapes  and  magnitude.  Thus  a 
five-run  analysis  essentially  provides  the  same  information 
as  a  20-run  analysis  but  at  significantly  reduced  computer 
cost.  To  conserve  further  on  computer  costs,  the  update 
period  is  increased  from  0.04  to  0.1  seconds.  Again,  it  is 
easiest  to  justify  this  by  comparing  Figures  4-4  and  4-6. 

Note  that  both  figures  have  similar  shapes  and  magnitudes. 
Therefore,  the  trends  of  the  tuning  parameters  R,  Q,  and  tau 
can  be  observed  from  a  five-run  analysis  with  an  update  of 
0.1  seconds  as  effectively  as  a  20-run,  0.04  update  Monte 
Carlo  anlysis.  Initially,  it  was  thought,  once  the  final 
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tuning  parameters  were  selected  based  Figure  4-4  on  a 
five-run  analysis  with  update  time  of  0.1  seconds,  the  number 
of  runs  and  update  time  could  be  reset  to  20  and  0.04  seconds 
respectively  to  verify  performance.  In  this  manner,  only  a 
relatively  small  number  of  large  Monte  Carlo  runs  need  be 
completed.  However,  as  shown  in  Figures  4-4  and  4-6, 
increasing  the  update  period  to  0.1  seconds  causes  a  change 
in  the  velocity  mean  error.  This  implies  the  update  period 
is  actually  a  tuning  parameter,  similar  to  R,  Q,  and  tau.  To 
illustrate  this,  the  same  simulation  is  rerun  at  an  update 
period  of  0.1  seconds  (see  Figure  4.7)  but  with  Q  arbitrarily 
decreased  by  a  factor  of  2.5  (  since  the  update  period  is 
increased  by  a  factor  of  2.5).  Figure  4-7  indicates 
performance  closer  to  Figure  4-4  than  Figure  4-6.  Thus,  as 
determined  from  observed  performance,  the  filter  will  have  to 
be  fine  tuned  whenever  the  update  period  is  changed. 
Analytically,  as  shown  in  Equation  (3-50),  is  a  function 
of  the  update  period,  At.  Thus  the  update  period  is  actually 
a  tuning  parameter  and  should  be  considered  during  follow-on 
aircraft  implementation.  But,  for  the  purpose  of  this  study, 
the  majority  of  the  simulations  are  completed  at  an  0.1 
update  period  to  conserve  on  computer  costs.  The  difference 
between  the  various  figures  are  detailed  numerically  in 
subsections  under  Section  5.3.1. 


4.3.3  SOFE  Modification 


In  order  to  use  SOFE  for  this  particular  problem,  a 

modification  is  made  to  the  basic  SOFE  program.  This  is 

necessary  because  the  dynamic  model  for  this  research  is 

referenced  to  the  antenna  reference  frame  (i  i  ,k  ) ,  which 

0,0’  o’ 

is  a  body  frame,  not  an  inertial  frame.  The  Kalman  filter 
propagation  from  time  ti_^  +  to  t^-  is  accomplished  in  an 
inertial  type  frame  (based  on  the  last  t^_^  +  estimates),  and 
then  x(t^  )  and  P(t^  )  are  impulsively  rotated  to  the  body 
frame  axis  at  time  t^  SOFE  is  modified  to  perform  the 
required  rotations.  Mathematically,  both  the  terms  in  the 
right-hand  side  of  Equations  (3-5)  and  (3-6)  must  be  in  the 
same  frame.  At  time  the  state  reference  is  propagated 

to  time  t^”.  If  the  state  reference  is  redefined  because  of 
any  change  in  heading,  pitch,  or  roll  during  the  propagation 
cycle,  the  required  information  at  time  t^-  must  be  rotated 
into  the  new  reference  frame  at  time  t^  in  order  to  apply  the 
update  relations  properly.  This  modification,  along  with 
subroutine  calls,  are  included  in  Appendix  D. 

4.4  Stand-Alone  Simulation  (SAS)  -  Simulation  and  Testing 
4.4.1  Introduction 

The  purpose  of  the  equivalent  discrete-time  models  and 
Kalman  filter  development  in  previous  chapters  is  to 
determine  how  well  a  discrete-time  filter  performs. 
Additionally,  as  previously  discussed  in  Section  3.3,  the 
filter  implementation  on  the  F-4E/G  will  be  an  equivalent 


discrete-time  design.  Thus,  the  SAS  program  permits  analysis 
of  a  discrete-time  filter  while  also  containing  the  basic 
code  required  for  implementation.  The  SAS  program  developed 
in  this  thesis  is  included  in  Appendix  E. 

4.4.2  Equivalent  Discrete-Time  Algorithm  Design  Considerations 
In  simplistic  terms,  propagation  equations  (Equations 
(3-48)  and  (3-49))  and  update  equations  (Equations  (3-4), 

(3-5),  and  (3-5))  are  implemented.  Due  to  limited  memory 
storage  (4k  or  possibly  8k  words),  desired  update  period 
(0.04  to  0.1  seconds),  and  limited  arithmetic  capability  of 
the  F-4E/G  fire  control  computer,  a  conventional  Kalman 
filter  algorithm  utilizing  matrix  operation  routines  is 
selected  for  the  preliminary  design.  Matrix  routines  are 
initially  used  in  the  SAS  program  for  simplicity,  but  if  the 
nine-state  filter  designed  is  implemented  on  the  F-4E/G,  the 
matrix  routines  should  be  simplified  by  taking  advantage  of 
the  number  of  zero  elements  and  matrix  symmetry. 

Additionally,  the  SAS  measurement  update  algorithm  should  be 
modified  into  a  U-D  filter  form  to  increase  numerical 
stability  (5:391-396).  Another  reason  for  using  a  U-D  filter 
is  to  avoid  as  many  divides  and  square  roots  as  possible.  A 
divide  function  on  the  F-4E/G  computer  consumes  24.33  ;usec 
while  a  square  root  consumes  150.0  ^usec,  which  can  rapidly 
use  up  the  available  40  to  100  msec  processing  time 
available.  As  a  comparison,  other  commonly  used  mathematical 
functions  such  as  addition,  subtraction,  and  multiplication 
consume  9.0  yusec  or  less.  Appendix  F  contains  calculations 


which  show  the  U-D  filter  satisfies  the  system  limitations 
defined  in  Section  1.3.  Alternate  forms  of  update  algorithms 
such  as  the  Potter  covariance  square  root,  Carlson  square 
root,  and  inverse  covariance,  probably  should  not  be 
considered  because  of  either  the  required  number  of  divides 


or  square  roots . 

4.4.3  Determining  Filter  Performance 

SAS  filter  performance  and  filters  simulated  using  SOFE 
are  evaluated  in  the  same  manner  (see  Section  4.3.2).  The 
SAS  program  stores  the  required  data  to  generate  plots 
through  a  postprocessor  similar  to  those  generated  in  the 
SSDC  processs.  Again,  the  easiest  way  to  demonstrate 
performance  is  to  examine  plotted  output.  Figures  4-6  and 
4-8  illustrate  the  simulated  performance  for  the  filter  using 
SSDC  and  the  SAS  process  respectively  (for  the  same  beam 
attack  trajectory).  Note  the  performance  is  essentially  the 
same;  the  performance  is  compared  numerically  in  subsections 
of  Section  5.3.1. 

4.5  Filter  Tuning  Philosophy  and  Methods 

Filter  tuning  is  required  to  achieve  the  best  filter 
performance,  compensating  for  the  modeling  approximations 
made  during  reduced  order  filter  design.  As  previously 
noted,  four  factors  can  be  varied  to  affect  filter  tuning. 
These  are  the  R  matrix,  the  2  matrix,  the  value  of  tau  used 
in  the  acceleration  filter  states,  and  the  update  period.  In 


;ure 


this  thesis,  R  (see  Equation  (2-34))  is  treated  as  a  constant 
(after  initial  testing  and  verification),  lumping  together 
system  and  radar  antenna  lag  noise.  Then,  Q  (see  Equation 
(3-42))  and  values  of  tau  (see  Equation  (2-10))  are  varied  to 
achieve  the  best  tuned  performance  (some  comparison  is  made 
for  the  effect  of  the  update  period). 

For  this  particular  problem,  good  estimates  of  target 
velocity  are  desired  more  then  precision  in  estimation  of 
position  and  acceleration  states  (24).  Thus,  the  philosophy 
employed  is  to  achieve  a  minimum  velocity  error  while 
maintaining  errors  in  position  and  acceleration  less  than  the 
corresponding  values  from  the  Wiener-Hopf  filter. 


V.  SIMULATION  RESULTS 


5 . 1  Introduction 

The  development  to  this  point  is  based  on  determining 
the  feasibility  of  replacing  the  existing  F-4E/G  Wiener-Hopf 
target  estimation  filter  with  a  Kalman  filter.  To  accomplish 
this,  a  preliminary  nine-state  Kalman  filter  is  designed  and 
tested  through  simulation  for  comparison  to  the  Wiener-Hopf 
filter.  This  chapter  includes  the  results  of  the  computer 
simulations  performed  on  the  preliminary  design.  As  will  be 
shown,  the  results  indicate  that  the  preliminary  design 
significantly  outperforms  the  Wiener-Hopf  filter  for  the  beam 
shot  attack.  Data  for  the  Wiener-Hopf  filter  performance  on 
the  tail  chase  is  not  available  and  is  not  directly  compared 
to  the  Kalman  filter  results.  However,  even  with  increased 
performance  on  the  beam  attack,  further  testing/remodeling  is 
desirable  to  provide  additional  performance  enhancement. 
Overall,  it  can  be  said  this  study  demonstrates  the  Kalman 
filter  is  a  feasible  choice  for  further  study/testing  to 
replace  the  Wiener-Hopf  filter  eventually. 

5.2  Preliminary  Kalman  Filter  Design 

The  preliminary  design  Kalman  filter  contains  nine 
states  and  uses  six  measurements  for  the  update.  The  states 
are  described  in  Section  2.4.3  and  the  measurements  in 
Section  2.5.2.  Linear  dynamics  are  employed  for  filter 
propagation  and  nonlinear  measurements  are  used  for  the 
filter  update,  as  described  in  Section  2.2. 
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5.3  Computer  Simulation  Results 


The  computer  simulation  methodology  is  discussed  in 
Chapter  U.  Four  different  simulation  groups  are  summarized 
in  this  chapter.  First,  the  beam  trajectory  is  tested 
extensively  since  it  provides  the  most  challenge,  as  it 
contains  the  largest  errors  in  velocity  estimates. 
Additionally,  a  direct  comparison  to  the  Wiener-Hopf  filter 
performance  is  possible.  Second,  the  tail  chase  is  briefly 
studied  to  demonstrate  how  the  tuning  values  derived  for  the 
beam  attack  trajectory  perform  in  the  tail  chase  scenario. 
Third,  "off-line"  (see  Section  3.1)  adaptive  tuning  is  tested 
to  show  that  further  study  of  on-line  adaptive  estimation  is 
warranted  and  desirable.  Finally,  it  is  determined  that 
overall  Kalman  filter  performance,  while  superior  to  the 
Wiener-Hopf  filter,  is  less  than  desired.  Therefore,  the 
last  simulation  group  attempts  to  isolate  the  major  source  of 
performance  degradation  and  to  provide  insight  where  future 
study  should  start. 

5.3.1  Beam  Attack  Simulation 


Plots  illustrating  Wiener-Hopf  filter  performance  for 
the  beam  attack  were  provided  by  OO-ALC/MMECB  (22)  and  are 
included  in  Appendix  G.  These  plots  are  used  as  a  baseline 
of  performance  from  which  enhanced  performance  is  desired. 
Appendix  G  also  includes  nine  plot  sets  from  simulation  runs 
of  the  same  trajectory  using  the  preliminary  design  Kalman 
filter.  The  plot  set  figure  numbers  correspond  to  the  table 
numbers  of  this  chapter.  For  example,  Figure  G.2.3.a  is 


interpreted  as 
G:  Appendix  G 

2:  Plot  Set  2  (also  indicates  data  is  in  Table  V-2) 

3:  Third  column  of  tabulation  in  Table  V-2 
a:  x  position  error  (b:  x  velocity,  c:  x  acceleration,  d: 
y  position,  e:  y  velocity,  f:  y  acceleration,  g:  z 
position,  h:  z  velocity,  and  i:  z  acceleration  errors) 
(Note,  that  x,y,z  axes  on  the  plot  sets  are  *-0>j0>k0 
axes,  not  space  coordinates.) 

Additionally,  units  for  all  the  tables  are 

feet  for  position  states  (x^,  x^,  and  x7), 

feet/second  for  velocity  states  (X2,  x^,  and  Xg), 

feet/second  for  acceleration  states  (x^,  x^,  and  x^), 
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feet  /second  for  Q,  and 
seconds  for  tau. 

SOFE/SOFEPL/DISSPLA/Calcomp  (SSDC)  simulation  is  primarily 
used  for  filter  evaluation  but  the  stand-alone  simulation 
(SAS)  program  (see  Section  4.4)  is  also  verified.  As 
discussed  in  Section  3.2.3,  R  is  not  varied  once  it  is 
initially  selected  (through  analysis  in  Section  2.5.2  and 
simulation  verification).  The  first  plot  set  is  presented  to 
show  how  a  range  of  Q  is  initially  selected.  With  a  Q  range 
selected,  plot  set  two  demonstrates  how  tau  is  selected. 

Plot  set  three  then  illustrates  how  a  final  value  of  Q  is 
selected.  With  the  values  of  Q,  R,  and  tau  selected,  the  SAS 
program  results  are  included  in  plot  set  four  to  demonstrate 


the  SAS  program  equivalence  to  the  SOFE/SOFEPL/DISSPLA/ 
Calcomp  process.  Finally,  plot  set  five  compares  a 
six-measurement  tuned  filter,  the  same  filter  with  four 
measurements  (dropping  azimuth  and  elevation  rate 
measurements),  and  the  Wiener-Hopf  filter  baseline 
performance.  Note  that  either  the  maximum,  the  minimum,  the 
average,  the  standard  deviation,  or  a  combination  thereof,  of 
the  mean  error  values  from  the  plotted  output  of  Appendix  G 
are  included  in  the  tables.  Other  representations  are 
possible,  but  the  mean  error  is  tabulated  because  of  the 
minimum  velocity  error  tuning  policy  established  in  Section 
4.5.  The  results  of  the  plot  sets  are  tabulated  in  the 
following  subsections. 

5. 3. 1.1  Plot  Set  One  -  Selecting  a  Q  Range 


Plot  set  one  is  used  to  select  an  initial  value  Q  range 
for  tuning  purposes.  Following  the  tuning  policy  of  Section 
4.5,  velocity  errors  are  to  be  minimized  instead  of  position 
or  acceleration  errors.  Additionally,  a  reasonable  settling 
time  to  essentially  zero-mean  error  for  all  the  states  is 
desired.  For  the  beam  shot  trajectory  analyzed  in  this 
research,  12  seconds  total  elapsed  time  is  selected  for  all 
states  to  return  to  approximately  zero-mean  error.  This 
corresponds  to  a  simulation  time  of  six  seconds  after  the 
fighter  stops  maneuvering  and  three  seconds  after  the  target 
stops  maneuvering.  Tau  of  0.2  seconds  is  initially  used 
based  on  previous  simulation  results  (not  included,  but  the 


effects  of  tau  are  demonstrated  in  plot  set  two).  P  is 

— O 

doubled  from  that  calculated  in  Equation  (3-43),  based  on 
previous  simulation  results  (not  included,  but  Appendix  G 
filter-computed  covariance  curves  are  observed  to  be 
reasonable).  Plot  set  one  results  are  condensed  in  Table 
V-l. 


Table  V-l 


Selection 

of  Q  Range 

(Maximum  Mean  Error 

Magnitude 

/  Elapsed 

Time  to  Settling) 

Q  values 

(tau=0.2,  update  period 

0.1  seconds) 

State 

373.25 

3732.5 

37325. 

373250 

3732500 

X1 

700/- 

750/- 

700/7.1 

560/7.4 

600/8.0 

x2 

170/- 

130/- 

100/12 

310/6.4 

800/7.4 

x3 

96/9.7 

96/9.2 

90/9.1 

95/9.1 

200/7.0 

x4 

1600/- 

1600/- 

1500/7.0 

1300/7.0 

825/8.0 

x5 

325/- 

170/- 

270/12 

900/7.2 

2500/7.2 

x6 

60/9.4 

60/9.4 

60/9.3 

100/9.1 

500/6.8 

x7 

2600/- 

2400/- 

2250/- 

2150/ + 

1600/8.0 

x8 

225/- 

240/- 

425/- 

775/10.5 

1550/8.2 

x9 

30/6.7 

30/6.7 

30/6.7 

30/9.0 

30/9.0 

-  indicates 

state  not 

settled  at 

12  seconds 

+  indicates 

state  almost  settled 

at  12  seconds 

Note:  All 

data  points 

read  off  plots 

Heuristically ,  Increasing  Q  weights  the  incoming 
measurements  more  than  the  internal  filter  propagation.  In 
Table  V-l,  Q  is  varied  by  an  order-of-magnitude  or  a  factor 
of  10  to  expedite  the  selection  process  (this  is  why  the 
significant  figures  are  carried,  to  clearly  indicate  the 
order-of-magnitude  changes).  Note  that  with  the  lower  Q,  the 
mean  velocity  errors  (states  X2,  x^,  and  Xg)  are  smaller  but 
the  mean  position  errors  (states  x^,  x^,  and  Xj)  are  larger 
and  these  states  are  not  settled  by  12  seconds.  Raising  Q  to 
373250  increases  the  mean  velocity  errors  but  causes  a 
desired  reduction  in  both  the  mean  position  error  and 
settling  times.  Raising  Q  to  3732500  results  in  large 
increases  in  velocity  and  acceleration  errors  (for  example, 
velocity  state  x^  increases  by  a  factor  of  2.5  and 
acceleration  state  x&  increases  by  a  factor  of  5).  Thus, 
results  of  Table  V-l  indicate  that  the  best  velocity 
performance  and  reasonable  settling  time  of  about  12  seconds 
are  obtained  in  a  2  range  of  of  37325  to  373250.  This 
corresponds  to  an  order-of-magnitude  change  in  Q  and  is  in 
the  range  calculated  in  Equation  (3-42). 

5. 3. 1.2  Plot  Set  Two  -  Selection  of  Tau 

A  Q  value  of  373250  is  selected  out  from  the  Q  range 
established  in  the  last  section  for  fine  tuning  of  tau. 

Again,  following  the  tuning  policy  of  Sections  4.5  and 
5. 3. 1.2,  mimimal  velocity  error  and  reasonable  settling  time 
are  desired.  The  plotted  results  are  condensed  in  Table  V-2. 
Note  that  a  value  of  tau  equal  to  0.5  seconds  results  in  good 


settling  time  characteristics  but  at  the  expense  of  large  mean 
velocity  errors.  Decreasing  tau  to  0.143  (which  corresponds 
to  a  1 IT  of  7.0  in  Equation  (2-10))  increases  the  settling 
time  for  some  of  the  states  but  reduces  velocity  mean  errors. 
Decreasing  tau  further  results  overall  in  increased  settling 
times  and  approximately  a  75  feet/second  bias  in  state  x^ 
between  7  and  10  seconds.  Results  of  Table  V-2  indicate  that 
a  tau  of  0.143  results  in  the  best  velocity  performance  with 
most  of  the  states  almost  settled  by  12  seconds. 

Table  V-2 


Selection  of  Tau 

(Maximum  Mean  Error  Magnitude  /  Elapsed  Time  to  Settling) 


State 


Tau  Values  (§=373250,  update  period  0.1  second) 


0.167 


0.143 


X1 

550/8.2 

560/7.4 

572/7.4 

588/7.2 

x2 

670/7.4 

310/6.4 

251/7.7 

203/7.7 

x3 

300/7.0 

95/9.1 

114/9.2 

106/9.3 

x4 

1000/8.2 

1300/7.0 

1333/7.0 

1379/6.5 

x5 

1950/7.6 

900/7.2 

740/7.0 

620/6.5 

x6 

600/9.3 

100/9.1 

89/9.1 

83/9.1 

x7 

1750/8.1 

2150/12+ 

2116/12+ 

2152/12+ 

x8 

1450/9.0 

775/10.5 

705/12. 

640/12+ 

x9 

300/9.3 

30/8.3 

41/7.0 

31/7. 

°-I 

634/9.0 
119/9.0 
98/9.3 
L465/7.5 
415/12. 1 

72/9.1 

£213/12+ 

542/- 

30/7.0 


-  indicates  state  not  settled  at  12  seconds 
t  indicates  state  almost  settled  at  12  seconds 
1  indicates  state  is  biased  by  75  feet/second 
between  7  and  10  seconds 
Note:  data  points  read  off  plots 


5. 3. 1.3  Plot  Set  Three  -  Selection  of  Q 

With  a  tau  of  0.143,  Q  is  now  fine  tuned.  The  range  of 
Q  is  divided  into  increments  of  37325,  149300,  261275,  and 
373250  (calculated  by  subtracting  37325  from  373250,  dividing 
by  three  resulting  in  111,975.0,  and  adding  this  figure  to 
37325  successively  to  obtain  the  values  listed).  Again,  the 
best  velocity  performance  and  all  states  settled  by  12 
seconds  are  desired.  The  results  of  simulation  runs  based  on 
these  values  are  contained  in  Table  V-3.  Data  points  are 
obtained  from  listable  output  of  SOFEPL.  Table  V-3  indicates 
the  best  value  of  Q  is  149300.  Again,  the  plot  sets  must  be 
studied  for  settling  time  characteristics.  If  the  settling 
time  associated  with  a  particular  Q  is  considered  too  long, 
then  a  higher  Q  can  be  selected  to  shorten  the  settling  time. 
Additionally,  as  shown  in  plot  set  two,  tau  can  also  be 
varied  to  affect  settling  time,  or  as  is  later  shown  in  plot 
set  four,  the  update  period  can  be  decreased  to  0.04  seconds. 
However,  it  is  easily  observed  that  decreasing  the  settling 
time  increases  the  velocity  error.  The  tradeoff  here  can  be 
partially  overcome  by  adaptive  tuning  as  discussed  in  Section 
3.1  and  later  demonstrated  in  Section  5.3.3.  Finally,  for  2 
equal  to  149300,  plots  of  the  measurement  residuals  are 
included  in  Appendix  G  to  provide  insight  on  what  the 
residuals  are  doing  corresponding  to  the  state  estimate 
errors.  Note  that  when  the  state  errors  are  large,  the  angle 
and  rate  measurement  true  residuals  statistics  cross  over 


Table  V-3 


Selection  of  Q 
(Mean  ErrorsT 


Q 

value  (Tau=0. 

143,  update 

period  0.1 

seconds ) 

ate 

37325 

149300 

261275 

373250 

^-minimum 

-94.3 

-82.6 

-70.3 

-57.1 

-maximum 

722.5 

647.0 

609.0 

588.4 

-average 

105.2 

94.4 

91.5 

90.7 

-std  dev 

222.0 

190.1 

172.3 

160.9 

2  .  . 
-minimum 

-79.4 

-110.5 

-153.1 

-203.3 

-maximum 

111.2 

103.3 

101.3 

99.7 

-average 

-10.9 

-22.4 

-25.1 

-26.1 

-std  dev 

50.3 

51.9 

56.4 

61.7 

^-minimum 

-6.8 

-20.6 

-30.6 

-38.3 

-maximum 

92.4 

94.2 

98.1 

106.4 

-average 

45.8 

44.8 

43.2 

42.2 

-std  dev 

35.8 

34.4 

35.0 

36.2 

4 

-minimum 

-1573.9 

-1485.0 

-1426.4 

-1379.5 

-maximum 

405.0 

514.4 

569.9 

604.3 

-average 

-58.1 

-45.3 

-39.5 

-36.4 

-std  dev 

362.4 

346.1 

337.1 

330.9 

5 

-minimum 

-87.3 

-96.3 

-99.5 

-101.4 

-maximum 

210.2 

371.0 

506.8 

620.8 

-average 

63.4 

71.5 

75.5 

78.0 

-std  dev 

76.7 

110.8 

139.0 

162.7 

^-minimum 

-8.4 

-27.2 

-37.8 

-45.0 

-maximum 

65.4 

65.4 

75.2 

83.1 

-average 

17.2 

14.1 

12.6 

11.6 

-std  dev 

20.1 

20.3 

22.2 

24.7 

^-minimum 

-970.2 

-966.6 

-964.5 

-962.0 

-maximum 

2308.9 

2226.4 

2168.8 

2152.1 

-average 

336.2 

235.2 

208.4 

196.3 

-std  dev 

796.3 

751.6 

720.4 

697.6 

^-minimum 

-361.9 

-516.8 

-590.0 

-640.3 

-maximum 

81.7 

100.3 

127.0 

150.7 

-average 

-119.9 

-163.1 

-173.1 

-176.6 

-std  dev 

161.9 

206.5 

227.5 

243.5 

9 

-minimum 

-27.7 

-28.8 

-29.9 

-31.3 

-maximum 

1.7 

5.8 

14.1 

22.3 

-average 

-3.5 

-2.2 

-1.4 

0.7 

-std  dev 

7.4 

7.2 

8.4 

10.0 

the  filter  computed  one-sigma  curves.  The  large  errors  are 
attributed  to  a  combination  of  improper  filter  tuning  and 
modeling.  The  residual  information  can  be  used  to  either 
"retune"  the  on-line  filter  through  adaptive  estimation 
techniques  (16:Chapter  10)  or  to  recalculate  state  estimates 
through  ad  hoc  techniques  (as  suggested  later  in  Section 
6.2.3  for  future  study). 

5. 3. 1.4  Plot  Set  Four  -  Simulation  Equivalency 

Plot  set  four  demonstrates  the  degree  of  equivalence 
between  various  simulation  runs  completed.  First,  the  SAS 
results  (five-run  analysis)  are  compared  to  the  SSDC  results 
(also  a  five-run  analysis).  Second,  the  SSDC  for  the  five 
run  analysis  at  an  update  period  of  0.1  seconds  is  compared 
to  an  equivalent  twenty-run  analysis.  Next,  a  SSDC  analysis 
at  an  update  period  of  0.04  seconds  is  compared  to  the 
previous  plots.  Then,  a  20-run,  0.04  second  update  period 
run  is  compared  to  the  three  previous  plots.  Finally,  as 
discussed  in  Section  4.3.2,  Q  is  decreased  to  59720  along 
with  an  update  period  decrease  to  0.04  seconds  to  demonstrate 
the  update  period  effect  on  the  tuning.  The  results  are 
condensed  in  Table  V-4.  Results  of  Table  V-4  indicates  first 
that  the  SAS  program  provides  results  very  close  (within  two 
to  five  percent  for  each  state)  to  the  equivalent  SSDC  run. 
This  can  be  observed  by  comparing  data  columns  one  and  two. 
Data  column  number  three,  when  compared  to  two,  indicates 
that  increasing  the  runs  from  five  to  twenty  does  not 
significantly  change  the  information  that  is  used  in 


Table  V-4 


Equivalency  of  Simulations 

Number 

1 

2 

(Mean  Errors) 

3  4 

5 

6 

Type 

SAS 

SSDC 

SSDC 

SSDC 

SSDC 

SSDC 

Q  value 

149300 

149300 

149300 

149300 

149300 

59720 

tau( sec ) 

0.143 

0.143 

0.143 

0.143 

0.143 

0.143 

ud( sec ) 

0.1 

0.1 

0.1 

0.04 

0.04 

0.04 

Runs 

5 

5 

20 

5 

20 

5 

State 

X1  . 
mm 

-82 

-82.6 

-57.2 

-26.8 

-29.1 

-53.8 

max 

610 

647.0 

573.5 

495.9 

475.4 

554.5 

x2 

min 

-112 

-110.5 

-125.5 

-223.4 

-216.0 

-122.2 

max 

96 

103.3 

93.0 

60.3 

62.3 

73.9 

X3  . 
min 

-18 

-20.6 

-9.5 

-23.5 

-14.4 

-12.5 

max 

94 

94.2 

88.5 

94.0 

85.5 

89.9 

j  x4  . 
min 

-1400 

-1485.0 

-1436.0 

-1329.5 

-1329.5 

-1474.4 

i  max 

510 

514.4 

550.0 

649.3 

645.6 

578.4 

x5  . 
mm 

-100 

-96.3 

-79.0 

-55.4 

-57.8 

-62.2 

max 

360 

371.0 

367.1 

611.4 

618.4 

380.5 

x6  . 

1  mm 

-27 

-27.7 

-15.4 

-30.7 

-22.0 

-16.6 

max 

65 

65.4 

62.5 

61.1 

62.3 

62.1 

'  x7  . 
mm 

-940 

-966.6 

-954.3 

-1036.9 

-1015.5 

-1050.7 

max 

2140 

2220.4 

2171.1 

2047.5 

2194.9 

2146.8 

;  Xo 

j  min 

520 

-516.8 

-514.2 

-674.6 

-679.5 

-539.1 

'  max 

110 

100.3 

96.8 

112.5 

136.9 

65.5 

Xq 

j  min 

-28 

-28.8 

-24.8 

-33.9 

-26.3 

-25.9 

i  max 

8 

5.8 

8.0 

22.6 

19.3 

7.5 

ud=update 

min=minimum 

SAS-Stand-Alone  Simulation 
SSDC=S0FE/ SOFEPL/DISSPLA/ Calcomp 

max 

^maximum 

performing  the  tuning  process.  This  justifies  running  the 
majority  of  the  simulations  at  five  runs.  In  other  words, 
the  information  trend  used  to  vary  tuning  parameters  does  not 
contain  any  unexpected  changes  (as  also  can  be  observed  by 
studying  Figure  Sets  G.4.1  and  G.4.2).  It  is  difficult  to 
quantify  the  percentage  change  overall,  because  different 
calculation  methods  lead  to  different  results,  and 
calculations  must  be  done  for  all  nine  states.  Therefore,  to 
avoid  an  excessive  development  on  percentage  change  that  is 
not  directly  used  in  this  study,  it  is  only  required  to  note 
than  changing  from  20  to  5  runs  does  not  cause  any  unexpected 
changes  in  the  plotted  curve  outputs.  Data  columns  four  and 
five  again  justify  the  five-run  analysis.  Additionally,  when 
data  columns  four  and  five  are  compared  to  columns  two  and 
three  respectively,  they  indicate  that  a  significant  degree 
of  change  occurs  in  velocity  state  mean  errors  (approaching  a 
factor  of  two  for  X2  and  x^)  when  the  update  period  is 
changed  from  0.1  to  0.04  seconds.  To  demonstate  the  effect 
of  the  update  period  change  further,  data  column  six  is 
included.  For  this  demonstration,  it  is  decided  to  decrease 
the  update  period  back  to  0.04  seconds  along  with  a  2.5  times 
decrease  in  Q  and  compare  the  results  to  the  other  data 
columns.  Overall,  data  column  six  is  closer  in  performance 
to  data  columns  one  through  three  than  with  columns  four  and 
five.  Thus,  an  "equivalence"  is  demonstrated,  but  it  is 
realized  the  goal  to  reduce  computer  costs  (Section  4.3.2)  or 
on-line  processing  loading  by  increasing  the  update  period 


from  0.04  tO  0.1  seconds,  actually  results  in  the  introducion 
of  another  tuning  parameter.  However,  this  section 
demonstrates  the  tradeoffs  that  must  be  considered  later  in 
the  selection  of  an  operational  update  period  (assuming  a 
form  of  the  extended  Kalman  filter  will  be  implemented  on  the 
F-4E/G) . 

5. 3. 1.5  Plot  Set  Five  -  Filter  Comparison 

Plot  set  five  compares  the  tuned  preliminary  design 
Kalman  filter  with  six  measurements  (range,  range  rate, 
azimuth  angle,  elevation  angle,  azimuth  angle  rate,  and 
elevation  angle  rate)  without  adaptive  tuning  to  the  existing 
Wiener-Hopf  filter  with  six  measurements  and  to  a  preliminary 
design  Kalman  filter  with  four  measurements  (range,  range 
rate,  azimuth  angle,  and  elevation  angle).  All  filters  are 
tested  for  the  same  trajectory,  and  provide  approximately  a 
one-to-one  comparison.  The  SSDC  plots  included  are  run  at  an 
update  period  of  0.1  seconds  while  the  Wiener-Hopf  filter  is 
run  at  0.04  seconds  (as  shown  in  the  last  section  an 
"equivalent"  filter  can  be  found  at  different  update  periods 
by  adjusting  Q,  thus  this  is  not  a  limiting  factor).  The 
results  are  condensed  in  Table  V-5.  Table  V-5  indicates  the 
tuned  preliminary  design  Kalman  filter  significantly 
outperforms  the  Wiener-Hopf  filter  in  velocity  (approaching  a 
factor  of  two)  and  acceleration  estimates  (Wiener-Hopf 
position  estimates  were  not  provided  by  00-ALC). 

Additionally,  the  data  indicates  that  a  four-measurement 
update  filter  can  provide  comparable  filter  performance  to 


Filter  Performance 
(Mean  Errors) 


e 

Q=149300 

Wiener-Hopf 

Q=149300(4  meas) 

minimum 

-82.6 

n/ a 

-146.9 

maximum 

647.0 

n/a 

590.5 

average 

94.4 

n/ a 

56.0 

std  dev 

190.1 

n/a 

182.3 

minimum 

-110.5 

0 

-163.4 

maximum 

103.3 

167 

97.3 

average 

-22.4 

n/a 

-24.1 

std  dev 

51.9 

n/ a 

64.1 

minimum 

-20.6 

-75 

-20.0 

maximum 

94.2 

100 

98.1 

average 

44.8 

n/a 

35.8 

std  dev 

34.4 

n/ a 

35.3 

minimum 

-1485.0 

n/a 

-1856.2 

maximum 

514.4 

n/a 

552.9 

average 

-45.3 

n/a 

-44.2 

std  dev 

346.1 

n/ a 

389.5 

minimum 

-96.1 

-1125 

-123.2 

maximum 

371.0 

750 

273.9 

average 

71.5 

n/a 

56.1 

std  dev 

110.8 

n/a 

100.9 

minimum 

-27.2 

-280 

24.4 

maximum 

65.4 

280 

68.4 

average 

14.1 

n/a 

11.4 

std  dev 

20.3 

n/ a 

19.0 

minimum 

-966.6 

n/ a 

-1161.0 

maximum 

2226.4 

n/ a 

2286.6 

average 

235.2 

n/ a 

109.6 

std  dev 

751.6 

n/ a 

713.5 

minimum 

-516.8 

-500 

-576.9 

maximum 

100.3 

1450 

74.8 

average 

-163.1 

n/a 

-149.4 

std  dev 

206.5 

n/a 

212.3 

minimum 

-28.8 

-280 

-25.8 

maximum 

5.8 

280 

2.5 

average 

-2.2 

n/ a 

-2.2 

std  dev 

7.2 

n/a 

5.7 

not  available 


that  of  the  six-measurement  update  filter.  This  should  be 
considered  for  actual  implementation,  as  the  azimuth  and 
elevation  angle  rate  measurement  computations  of  Equation 
(3-5)  or  Equations  (3-30)  through  (3-39)  pose  a  heavy 
computational  burden  during  filter  update.  If  the  processing 
time  or  the  available  memory  becomes  a  serious  limitation 
during  implementation,  Equations  (3-30)  through  (3-39)  can  be 
eliminated  and  result  in  performance  similar  data  column 
three.  Finally,  note  that  for  all  states  the  average  of  the 
mean  error  is  not  zero  and  that  the  standard  deviation  is 
relatively  large.  This  is  attributed  to  the  beam  trajectory 
and  the  Kalman  filter  performance.  During  a  fighter 
maneuver,  the  filter  errors  are  not  zero-mean  (because  of 
degraded  filter  performance)  as  they  approach  when  the 
fighter  is  not  maneuvering.  This  is  easily  observed  by 
studying  the  plot  sets  in  Appendix  G. 

5.3.2  Plot  Set  Six  -  Tail  Chase  Simulation 

Results  from  a  brief  tail  chase  analysis  are  condensed 
in  Table  V-6.  The  purpose  of  the  tail  chase  simulation  is  to 
ascertain  how  the  tuned  values  for  the  beam  attack  trajectory 
perform  in  the  tail  chase  scenario.  Since  data  on  the  tail 
chase  performance  of  the  Wiener-Hopf  filter  was  not  available 
from  OO-ALC,  the  tail  chase  is  only  studied  to  gain  insight 
into  filter  performance.  Table  V-6  and  plot  set  six  indicate 
the  tuned  values  for  the  beam  attack  trajectory  result  in 
signicantly  biased  estimates  (for  states  x^  through  x^)  in 


WTiwoW 


Table  V-6 


Tail  Chase  Performace 

( Mean 

Errors ) 

Q  values  (Tau=0. 

143,  update  period 

0.1  seconds) 

149,300 

373,250 

149,300 

te 

R 

R 

Rnom 

minimum 

-17.8 

-18.0 

-22.2 

maximum 

43.3 

41.5 

33.2 

average 

-2.1 

-2.7 

-0.7 

std  dev 

7.9 

8.1 

5.9 

minimum 

-24.4 

-19.9 

-38.8 

maximum 

10.4 

18.7 

10.1 

average 

-5.4 

-1.9 

-4.7 

std  dev 

7.4 

7.9 

7.6 

minimum 

-81.1 

-107.6 

-73.3 

maximum 

26.4 

58.1 

25.5 

average 

-38.4 

-34.4 

-31.1 

std  dev 

24.2 

28.3 

22.9 

minimum 

-26.3 

-27.3 

-13.2 

maximum 

312.2 

288.7 

110.0  1 

average 

144.4 

136.4 

31.4  j 

std  dev 

86.1 

85.4 

21.5  j 

minimum 

-126.4 

-130.4 

-233.4  | 

maximum 

136.4 

171.1 

69.0  j 

average 

-10.2 

-10.5 

62.8  ! 

std  dev 

90.3 

81.8 

107.7  | 

minimum 

-2.7 

-6.7 

-6.3  ! 

maximum 

101.4 

109.1 

106.0  i 

average 

51.1 

53.6 

62.3  j 

std  dev 

26.7 

26.3 

25.1  | 

minimum 

-77.1 

-73.2 

j 

-18.0  | 

maximum 

365.2 

38.9 

80.9  1 

average 

155.5 

169.0 

23.5  1 

std  dev 

138.4 

148.7 

25.7  ! 

minimum 

-340.1 

-284.1 

-416.4  i 

maximum 

34.8 

43.4 

21.0  ! 

average 

-155.0 

-127.0 

-181.5 

std  dev 

134.2 

109.6 

183.0 

minimum 

-66.6 

-67.4 

-66.7 

maximum 

7.0 

16.1 

15.2  1 

average 

-25.8 

-18.6 

-11.6  j 

std  dev 

22.0 

20.2 

20.1 

the  tail  chase  scenario.  The  mean  error  values  for  states  x^ 
through  Xg  do  not  return  to  essentially  zero-mean 
characteristics  as  in  the  beam  attack  because,  once  the 
fighter  starts  its  maneuver,  it  continues  maneuvering  for  the 
remainder  of  the  simulation.  States  x^  and  x ^  exhibit  good 
performance  since  small  errors  in  azimuth  and  elevation 
angles  have  less  of  an  impact  on  these  states  due  to  problem 
geometry.  Better  performance  for  states  x^  through  Xg  may  be 
obtained  by  additional  tuning,  but  at  the  cost  of  degrading 
filter  performance  during  a  beam  attack.  This  implies  that 
for  implementation,  it  may  be  beneficial  to  change  the  tuning 
parameters  for  the  particular  trajectory  being  flown.  For 
example,  the  range  rate  could  be  used  as  a  test  to  determine 
which  set  of  tuning  parameters  to  use.  If  the  range  rate  is 
high,  (head-on  attack),  moderate  (beam  attack),  or  low  (tail 
chase  attack)  the  corresponding  best  tuning  parameters  could 
be  used  for  the  filter.  Alternately,  three  filters  with 
different  tuning  parameters  could  be  implemented 
simultaneously,  in  a  multiple  model  adaptive  estimator 
configuration  (16:129-136),  one  for  each  type  of  trajectory 
attack.  This  requires  more  computer  memory  and  operating 
time  over  a  single  filter  but  reduces  transient  type  behavior 
that  results  by  changing  tuning  parameters.  This  option 
should  not  be  discarded  as  overly  restrictive  until  further 
research  is  completed.  It  has  already  been  shown  that  a 
filter  with  four  meaurements  may  suffice  and  that  the  update 
period  can  be  increased  to  0.1  seconds  without  serious  filter 


degradation.  Thus,  it  may  be  possible  to  implement  more  than 
one  on-line  filter. 


5.3.3  Plot  Set  Seven  -  Off-Line  Adaptive  Estimation 

At  this  point,  even  with  increase  of  performance  of  the 
nine-state  Kalman  filter  over  the  Wiener-Hopf  filter,  the 
Kalman  filter  performance  should  be  improved.  The  remainder 
of  this  chapter  addresses  ways  of  possibly  improving  filter 
performance  while  simultaneously  searching  for  the  source(s) 
that  cause  the  filter’s  degradation.  Adaptive  estimation,  as 
discussed  in  Section  3.1,  is  one  method  of  possibly  enhancing 
filter  performance. 

The  purpose  of  off-line  adaptive  estimation  in  this 
research  is  to  demonstrate  that  further  research  in  the  area 
of  on-line  adaptive  estimation  is  warranted  and  desirable  by 
establishing  a  baseline  of  performance  that  an  on-line 
adaptive  estimation  filter  can  approach.  The  off-line 
adaptive  estimation  plot  set,  plot  set  seven,  is  simulated  by 
lowering  Q  from  373,3250  to  37,325  during  a  fighter  maneuver 
and  conversely  raising  Q  to  373,250  when  the  fighter  is  not 
maneuvering.  The  process  of  lowering  2  during  a  fighter 
maneuver  contradicts  the  expected  tuning  process.  Normally, 
during  a  fighter  maneuver,  measurement  uncertainties 
increase,  and  Q  is  increased  to  compensate  for  the  additional 
uncertainty  (16:121-129).  The  lowering  of  g  here  is 
attributed  to  truth  model  performance  (and  possibly  fighter 
performance  if  the  truth  model  accurately  models  the  fighter 


and  measurements  during  a  maneuver).  From  the  truth  model 
output,  measurement  errors  during  a  fighter  maneuver  are 
observed  to  approach  and  often  exceed  the  three-sigma  values 
of  the  R  matrix.  Thus  by  lowering  Q,  more  weight  is  placed 
on  the  dynamics  model  until  the  measurements  become  reliable. 
The  SAS  program  is  used  for  the  off-line  adaptive  estimation 
because  of  difficulties  that  occur  in  integration  routines 
within  SOFE  when  Q  was  impulsively  reset  to  a  different 
value.  The  off-line  adaptive  estimation  run  is  then  compared 
to  equivalent  nonadaptive  SOFE  runs  with  the  high  and  low  Q 
values.  The  results  of  the  simulation  are  condensed  in  Table 
V-7.  Table  V-7  indicates  that  adaptive  estimation  helps  in 
reducing  position,  velocity,  and  acceleration  errors. 

Velocity  states  x2,  x^,  and  Xg  for  the  off-line  adaptive 
estimation  have  better  settling  times  than  those  of  a 
constant  Q  of  37,325  and  smaller  errors  then  those  for  a 
constant  Q  of  373,250.  Additionally,  settling  times  of 
position  states  x^,  x^,  and  x-j  are  slightly  worse  than  either 
of  the  nonadaptive  runs.  Finally  the  acceleration  states 
(X3,  Xg,  and  Xg)  mean  error  adaptive  values  fall  in  between 
the  simulation  runs  when  2  equals  37,250  and  373,250.  Thus, 
for  the  beam  attack  trajectory,  off-line  adaptive  estimation 
improves  filter  performance.  Further  study  in  on-line 
adaptive  estimation  is  warranted  and  desirable. 


Table  V-7 


State 


Adaptive  Estimation  Comparison 
(Mean  Errors  /  Elapsed  Time  to  Settling) 

(Tau=0.143,  update  period  0.1  seconds) 
Q=37 , 325  Q=variable  Q=373,250 

SOFE  SAS  SOFE 


-minimum 

-maximum 

-94. 3/8. 31 
722.5 

-94/9.2 

580 

-57.1/7.4 

588.4 

-minimum 

-maximum 

-79.4/12+ 

111.2 

-136/11.0 

56 

-203.3/8.8 

99.7 

-minimum 

-maximum 

-6.8 

92.4 

-38/9.3 

95 

-38.3/9.3 

106.4 

-minimum 

-maximum 

-1573. 9/8. 01 
405.0 

-1230/8.5 

600 

-1379.6/6.8 

604.3 

-minimum 

-maximum 

-87.3/- 

210.3 

-50/11.0 

420 

-101.4/9.0 

620.8 

-minimum 

-maximum 

-8. 4/9. 3 
65.4 

-32/9.3 

65 

-45.0/9. 

83.1 

-minimum 

-maximum 

-970. 2/-1 
2308.9 

-960/12+ 

2150 

-962.0/12 

2152.0 

-minimum 

-maximum 

-361.9/- 

81.7 

-420/12+ 

81 

-640.3/12 

150.7 

-minimum 

-maximum 

-27.7/6.7 

1.7 

-24/6.7 

12 

-31.3/6. 

22.3 

-  =  not  settled  at  12  seconds 
t  =  almost  settled  by  12  seconds 

1  =  biased,  mean  error  value  approaches  or  exceeds  the 
filter-computed  covariance  value 


5.3.4  Isolation  of  Factors  That  May  Cause  Filter  Degradation 


Even  with  the  increased  filter  performance  obtained 
through  adaptive  estimation,  overall  filter  performance 


enhancement  is  still  desired.  It  is  now  beneficial  to 
isolate  the  cause  of  filter  degradation.  Reduced  filter 
performance  could  be  caused  by  poor  measurements,  incomplete 
filter  modeling,  or  incomplete  truth  state  modeling.  Each  of 
these  are  studied  in  the  following  subsections. 

5. 3.4.1  Plot  Set  Eight  -  Measurement  Lag  Removed 

The  quality  of  the  measurements  received  from  the  truth 
model  can  cause  reduced  filter  performance.  Theoretically, 
if  the  dynamic  lag  of  the  radar  is  removed  from  the 
simulation,  the  filter  performance  should  improve.  To  test 
this,  the  trajectory  generation  program  is  rerun  without 
radar  antenna  lag,  resulting  initially  in  perfect 
measurements.  Then,  either  nominal  noise,  Rnom  of  Equation 
(2-3)  or  the  R  of  Equation  (2-33)  (the  R  used  in  the  tuned 
filter)  is  added  to  the  measurements  in  SOFE.  Data  for  each 
of  these  runs  is  condensed  in  Table  V-8.  Table  V-8  indicates 
the  filter  does  not  significantly  improve  when  the  dynamic 
lag  of  the  radar  is  removed  and  R  of  Equation  (2-34)  is  used. 
Using  Rnom  of  Equation  (2-3)  is  not  a  fair  comparison  because 
Q  should  be  changed  for  tuning  and  then  compared.  Thus, 
misrepresentation  of  dynamic  lags  in  the  measurements  can  be 
dismissed  as  the  major  cause  of  filter  degradation.  This 
implies  the  modeling  approximations  made  or  the  incomplete 
acceleration  state  model  may  now  be  considered  as  the  cause 
of  reduced  filter  performance. 


Table  V-8 


Measurement  Lag  Removed 
(Mean  Errors) 


2=149300 


no  lag 

no  lag 

lag 

ate 

Tuned  R 

Nominal  R 

Tuned  _ 

^"-minimum 

-181.5 

-227.7 

-82.6 

-maximum 

592.3 

284.0 

647.0 

-average 

32.2 

20.1 

94.4 

-std  dev 

161.2 

72.9 

190.1 

^-minimum 

-158.3 

-409.0 

-110.5 

-maximum 

72.5 

98.3 

103.3 

-average 

-25.5 

-28.1 

-22.4 

-std  dev 

60.4 

121.2 

51.9 

3  .  • 

-minimum 

-20.1 

-21.7 

-20.6 

-maximum 

98.1 

101.5 

94.2 

-average 

35.9 

37.5 

44.8 

-std  dev 

35.4 

37.2 

34.4 

-minimum 

-1124.0 

-707.6 

-1485.0 

-maximum 

626.1 

714.5 

514.4 

-average 

28.5 

23.5 

-45.3 

-std  dev 

259.5 

194.6 

346.1 

^-minimum 

-51.1 

-276.0 

-96.1 

-maximum 

316.1 

1009.7 

371.0 

-average 

66.7 

29.7 

71.5 

-std  dev 

100.6 

218.9 

110.8 

^-minimum 

-24.4 

-39.0 

-27.2 

-maximum 

68.4 

64.2 

65.4 

-average 

11.4 

6.1 

14.1 

-std  dev 

19.0 

20.4 

20.3 

^-minimum 

-726.8 

-561.0 

-966.6 

-maximum 

1797.2 

685.1 

2226.4 

-average 

82.0 

7.2 

235.2 

-std  dev 

588.1 

229.7 

751.6 

^-minimum 

-603.9 

-1594.2 

-516.8 

-maximum 

158.9 

482.6 

100.3 

-average 

-156.4 

-178.6 

-163.1 

-std  dev 

234.6 

471.7 

206.5. 

9 

-minimum 

-26.0 

-47.4 

-28.8 

-maximum 

4.0 

39.9 

5.8 

-average 

2.3 

0.0 

-2.2 

-std  dev 

5.9 

14.9 

7.2 

5. 3.4.2  Plot  Set  Nine  -  Acceleration  and  Truth  Model  Testing 
To  support  the  idea  that  the  first  order  Gauss-Markov 
acceleration  model  may  be  causing  reduced  filter  performance, 
the  following  hypothesis  is  made.  The  commonly  used  value  of 
tau  equal  to  0.5  seconds  in  the  first  order  Gauss-Markov 
zero-mean  acceleration  model  for  fighter  type  targets  (17) 
does  not  provide  adequate  performance.  A  tau  of  0.143 
seconds  provides  superior  performance.  In  other  words,  to 
compensate  for  incomplete  modeling,  tau  is  changed.  The 
value  of  tau  affects  filter  performance  most  when  the  fighter 
maneuvers.  Thus  the  value  of  tau  which  is  part  of  the  target 
acceleration  model  is  changed  to  compensate  for  fighter 
acceleration,  a  compensation  for  incomplete  modeling. 

To  test  the  above  hypothesis,  two  new  beam 
trajectories  are  generated.  First  the  original  beam 
trajectory  is  modified  so  there  are  no  fighter  maneuvers. 
Then,  the  original  trajectory  is  modified  again  so  the  target 
does  not  accelerate.  In  this  manner,  it  is  possible  to 
isolate  whether  incomplete  target  acceleration  modeling  or 
the  truth  model  is  causing  reduced  filter  performance.  The 
results  of  SSDC  runs  using  the  above  described  trajectories 
are  condensed  in  Table  V-9.  The  results  of  Table  V-9  and  the 
associated  plots  show  that  the  filter  performs  well  when 
there  are  no  fighter  maneuvers  (based  on  the  minimal  velocity 
and  settling  performance  criteria  of  Sections  4.5  and 
5. 3. 1.2).  On  the  other  hand,  the  filter  reverts  to  degraded 
performance  when  the  fighter  rolls  and  the  target  does  not 


Table  V-9 


Model 

Testing 

( Mean 

Error ) 

Q= 

149300  (Tau=0.143 

,  update  period 

State 

no  fighter  man 

no  target  man 

X1 

-minimum 

-24.4 

-162.4 

-maximum 

130.9 

538.0 

-average 

38.9 

43.9 

-std  dev 

41.2 

170.2 

x2 

-minimum 

-33.1 

-196.0 

-maximum 

104.7 

18.8 

-average 

33.9 

-59.7 

-std  dev 

37.1 

71.7 

x3 

-minimum 

-19.7 

-26.4 

-maximum 

94.2 

34.4 

-average 

41.8 

1.4 

-std  dev 

33.2 

37.2 

x4  .  . 
-minimum 

-131.7 

-1432.9 

-maximum 

46.4 

629.4 

-average 

-37.6 

2.8 

-std  dev 

40.8 

353.8 

x5 

-minimum 

-89.2 

-35.4 

-maximum 

28.9 

424.9 

-average 

-23.5 

92.7 

-std  dev 

31.1 

126.3 

x6 

-minimum 

-27.3 

-21.5 

-maximum 

72.9 

30.4 

-average 

16.7 

0.7 

-std  dev 

22.6 

9.2 

x7 

-minimum 

-80.5 

-995.6 

-maximum 

150.6 

2181.2 

-average 

33.0 

216.3 

-std  dev 

53.0 

724.3 

x8 

-minimum 

-33.3 

-527.1 

-maximum 

54.0 

40.0 

-average 

8.2 

-175.4 

-std  dev 

19.6 

194.2 

x9 

-minimum 

-2.0 

-15.4 

-maximum 

2.6 

8.8 

-average 

0.2 

0.0 

-std  dev 

0.9 

3.9 

1  seconds) 
both  man 

-82.6 

647.0 

94.4 

190.1 

-110.5 

103.3 

-22. Ur 

51.9 

-20.6 

94.2 
44.8 

34.4 

-1485.0 

514.4 
-45.3 

346.1 

-96.1 

371.0 

71.5 

110.8 

-27.2 

65.4 

14.1 

20.3 

-966.6 

2226.4 

235.2 
751.6 

-516.8 

100.3 
-163.1 

206.5 

-28.8 

5.8 

-2.2 

7.2 


i 


accelerate.  This  implies  either  the  rotations  before  the 
measurement  update  (see  Section  4.3.3  and  Appendix  D)  or  the 
truth  model  is  causing  filter  degradation.  The  rotations 
used  are  dismissed  as  suspect  for  several  reasons.  First, 
they  have  been  checked  for  proper  implementation.  Second, 
after  the  fighter  stops  maneuvering,  the  filter  recovers, 
which  indicates  some  degree  of  correctness.  Third,  a 
concurrent  research  effort  studying  the  F-4E/G  long  range 
intercept  problem  (4)  using  a  completely  inertial  model  (thus 
avoiding  the  rotations  in  question)  also  experiences  similar 
filter  degradation  when  using  an  equivalent  truth  model. 
Finally,  the  Wiener-Hopf  filter  is  experiencing  a  similar 
problem,  in  both  simulation  and  actual  implementation.  The 
one  common  simulation  element  is  the  truth  model.  Thus,  as  a 
next  logical  step,  it  is  recommended  that  the  truth  model  be 
revalidated,  to  ensure  a  problem  has  not  been  designed  into 
the  truth  model  that  is  causing  the  simulated  Kalman  filter 
degradation.  Alternatively,  a  test  flight  could  be  flown 
using  the  nine-state  Kalman  filter  to  determine  if  the 
problem  exists  in  the  real  world  for  the  Kalman  filter  as  it 
does  for  the  Wiener-Hopf  filter.  If  it  is  determined  the 
problem  is  real  for  the  Kalman  filter,  an  ad  hoc  procedure 
based  on  the  measurement  residuals  that  may  enhance  the 
nine-state  EKF  performance  is  suggested  for  future  research 
in  the  next  chapter. 


5.4  Simulation  Implications 


Overall,  this  chapter  demonstrates  that  the  simulation 
results  of  the  preliminary  design  nine-state  extended  Kalman 
filter  exceeds  the  performance  of  the  current  Wiener-Hopf 
filter.  As  such,  the  preliminary  design  should  be  continued 
or  expanded  into  a  final  design.  The  next  chapter  summarizes 
the  preliminary  design  and  recommends  steps  that  should  be 
considered  in  developing  the  final  design. 


VI.  CONCLUSIONS  AND  RECOMMENDATIONS 


6 . 1  Conclusions 

6.1.1  Problem  Review 

Currently,  the  F-4E/G  uses  a  Wiener-Hopf  filter  for 
estimating  target  position,  velocity,  and  acceleration  during 
air  combat  maneuvering.  As  implemented,  the  errors  between 
the  actual  target  variables  and  the  estimate  of  these 
variables  are  too  large.  The  purpose  of  this  study  is  to 
evaluate  the  feasibility  of  replacing  the  Wiener-Hopf  filter 
with  a  Kalman  filter  in  order  to  obtain  better  estimates. 

The  evaluation  is  made  by  first  designing  an  appropriate 
Kalman  filter  and  then  testing  the  design  through  computer 
simulation.  The  computer  simulation  results  indicate  that 
the  Kalman  filter  significantly  outperforms  the  Wiener-Hopf 
filter.  Thus,  the  Kalman  filter  is  a  feasible  choice  for 
replacing  the  Wiener-Hopf  filter. 

6.1.2  Design  Review 

The  extended  Kalman  filter  developed  is  a  preliminary 
design  for  making  the  evaluation  described  above.  The  Kalman 
filter  contains  nine  states  (three  relative  target  position, 
three  total  target  velocity,  and  three  total  target 
acceleration  states).  Filter  propagation  is  based  on  linear 
time-invariant  dynamics  primarily  because  of  the  limited 
capabilities  of  the  on-board  aircraft  computer.  The  linear 
dynamics  permits  propagation  by  a  state  transition  matrix, 
resulting  in  a  computationally  efficient  implementation. 
Measurement  updates  use  six  measurements  (range,  range  rate, 


azimuth  angle,  elevation  angle,  azimuth  rate,  and  elevation 
rate)  available  on  the  F-4.  An  extended  Kalman  filter  is 
used  since  nonlinear  measurement  equations  result  when  the 
measurments  are  expressed  in  terms  of  the  states.  Both 
continuous  time  sampled-data  and  discrete-time  sampled-data 
designs  are  included. 

6.1.3  Results  Review 

The  continuous  time  sampled-data  design  is  used  in 
validating  filter  performance  through  a  Monte  Carlo  analysis 
using  a  computer  aided  design  package  called  Simulation  for 
Optimal  Filter  Evaluation  (SOFE).  The  equivalent 
discrete-time  design  is  also  used  in  validating  filter 
performance,  and,  since  it  is  a  discrete-time  design,  it  can 
be  easily  implemented  on  the  F-4E/G  computer.  The  results  of 
the  simulation  indicate  the  following: 

1.  The  current  update  period  of  0.04  seconds  can  be 
increased  to  0.1  seconds  with  filter  retuning  and  still 
retain  "near  equivalent"  filter  performance  (see  Section 
5. 3. 1.4  and  Table  V-5). 

2.  The  discrete-time  design  and  simulation  provide 
results  very  similar  to  the  results  from  SOFE  for  the 
same  problem  parameters  (within  two  to  five  percent  for 
ail  states,  see  Table  V-5). 

3.  The  Kalman  filter  significantly  outperforms  the 
Wiener-Hopf  filter  for  the  beam  attack  trajectory 


tested.  Position  state  information  is  not  available  for 
the  Wiener-Hopf  filter,  but  for  velocity  and 
acceleration,  overall  performance  is  increased  by  more 
than  a  factor  of  two  as  shown  in  Table  V-4.  Other 
trajectories  are  not  directly  compared  since  Wiener-Hopf 
filter  data  was  not  available. 

4.  A  Kalman  filter  using  a  four-measurement  filter 
update  (range,  range  rate,  azimuth  angle,  and  elevation 
angle)  also  significantly  outperforms  the  Wiener-Hopf 
filter.  Again,  for  velocity  and  acceleration  states, 
overall  peformance  is  improved  by  more  than  a  factor  of 
two.  Compared  to  a  6-measurement  filter,  overall 
position  error  increases  11  percent.  As  explained  in 
Section  5. 3. 1.5,  a  four-measurement  filter  reduces  the 
computational  loading  and  may  allow  more  than  one 
on-line  filter. 

5.  The  tuned  filter  for  a  beam  attack  provided  biased 
estimates  for  a  tail  chase  scenario  (see  Table  V-6  and 
pages  G-172  through  G-180).  Thus,  as  explained  in 
Section  5.3.2,  more  than  one  on-line  filter  may  be 
desirable . 

6.  Results  of  off-line  adaptive  estimation  simulations 
show  on-line  adaptive  estimation  can  provide 
substantially  enhanced  performance  over  a  nonadaptive 
filter  (see  Table  V-7). 


7.  To  reduce  state  estimate  errors  further,  the  final 
tuned  filter's  performance  should  be  improved.  Possible 
sources  of  filter  degradation  are  poor  measurements, 
modeling  approximations,  or  errors  in  the  truth  model. 
Measurements  and  modeling  approximations  are  tested  by 
an  appropriate  choice  of  simulations  and  are  found  not 
to  be  a  major  source  of  degradation.  As  a  result,  doubt 
is  cast  on  the  truth  model  performance  which  implies,  as 
described  in  Section  5. 3. 4. 2,  that  the  trajectory 
generation  (truth  model)  should  be  revalidated. 


6 . 2  Recommendations 

The  preliminary  filter  designed  in  this  thesis  should 
be  pursued  as  viable  replacement  for  the  Wiener-Hopf  filter. 
Areas  where  additional  study  should  be  concentrated  to 
further  develop  the  Kalman  filter  can  be  divided  into  tuning, 
remodeling  and  testing  categories. 

6.2.1  Tuning 

To  enhance  the  Kalman  filter  performance  further, 
additional  tuning  should  be  considered.  Specific  areas  where 
additional  tuning  may  provide  enhancement  are: 


1.  Tuning  on  a  particular  aircraft  body  axis  (iQ,  jQ, 

kQ).  Because  of  the  problem  and  aircraft  geometry,  for 

the  trajectories  tested,  the  target  parameters  in  iQ 

change  much  slower  than  either  j  or  k  (an  aircraft's 

o  o 

roll  response  is  much  more  rapid  than  heading  response 


,NV4'-mv 


in  a  turn).  This  implies  further  testing  using 
different  values  of  Q  and  tau  for  different  axes  may  be 
beneficial . 

2.  Tuning  for  several  different  trajectories  (beam 
attack,  tail  chase,  and  head-on  attacks). 

3.  If  possible,  development  of  an  overall  nonadaptive 
tuned  filter  suitable  for  all  trajectories. 

4.  Development  of  an  on-line  adaptive  filter  to  tune 
adaptively  to  trajectory  changes. 

6.2.2  Testing 

Beyond  the  tuning  described  above,  further  testing  is 
required.  Areas  where  testing  may  be  beneficial  are: 

1.  Use  of  a  different  trajectory  generation  simulation 
(truth  model)  to  determine  if  filter  performance 
inproves . 

2.  Implementation  of  the  existing  discrete-time  design 
with  a  modified  update  algorithm  (using  a  U-D  update 
algorithm  as  discussed  in  Section  4.4.2)  on  an  OO-ALC 
test  aircraft  and  a  flight  test  performed  to  verify  the 
filter's  performance  in  the  real  world. 

3.  Addition  of  noise  to  the  inertial  measurement  unit 
(IMU)  information,  to  determine  the  affect  of  IMU  noise 
on  the  Kalman  filter  performance.  Chapter  V  simulation 


is  based  on  a  perfect  IMU.  Adding  noise  to  the  IMU 
information  will  affect  both  filter  propagation  by 
adding  noise  to  fighter  velocity  terms  in  Equation 
(2-10)  as  well  as  filter  update  equations  by  adding 
noise  to  rotation  terms  used  in  performing  the 
transformations  just  prior  to  the  measurement  update. 

4.  Additional  simulation  of  the  equivalent 
discrete-time  design  using  a  16-bit,  fixed  point,  word- 
length  configuration  to  determine  the  wordlength  effect 
on  the  Kalman  filter  performance.  The  discrete-time 
filter  update  algorithm  should  be  changed  to  a  U-D 
algorithm  for  numerical  stability. 

6.2.3  Remodeling 

Remodeling  may  be  required  if  the  above  retuning  and 
additional  testing  steps  result  in  performance  less  than 
desired.  Adding  two  states  to  the  propagation  model  for 
estimating  the  antenna  azimuth  and  elevation  errors  may  be 
necessary  if  these  error  signals  can  not  be  obtained  from  the 
aircraft  radar.  But,  as  discussed  in  paragraph  6.1.3,  item 
7,  the  present  simulated  "measurements"  do  not  appear  to 
cause  serious  filter  degradation. 

Finally,  ad  hoc  procedures  may  be  evaluated  for 
achieving  enhanced  filter  performance.  One  proposed 
procedure  is  based  on  noting  that,  when  the  fighter 
maneuvers,  the  state  errors  become  large  and  the  measurement 
residuals  for  angles  and  angle  rates  cross  over  their 


filter-computed  one-sigma  values.  If  a  fighter  maneuver  is 
detected  (from  the  IMU  accelerometers  or  fighter  control 
stick  position),  the  position,  velocity,  and  acceleration 
estimates  can  be  modified  by  calculated  increments  and  x(t^) 
recomputed  as  explained  below.  Using  residual  information 
when  it  is  outside  an  established  one-sigma  bound,  the 
corresponding  amount  over  the  one-sigma  can  be  added  back 
into  the  problem  geometry  as  unmodeled  displacements, 
velocities  and  accelerations.  A  position  correction  factor 
for  j  and  kQ  can  be  calculated  from  the  geometry  using  the 
i  position  estimate  and  the  residual  angle  over  the 
one-sigma  bound,  a  velocity  correction  term  calculated  by 
dividing  the  position  correction  by  the  update  period,  and  an 
acceleration  correction  by  dividing  the  velocity  correction 
term  by  the  update  period.  This  is  not  unreasonable,  since, 
as  shown  in  the  simulation,  position  errors  in  iQ  are  much 
smaller  than  jQ  and  kQ.  Additionally,  in  the  next 
propagation  cycle,  all  three  axes  will  be  affected  in 
performing  the  calculation  of  H[t^  ^(t^  )  ] .  The  recomputed 
SUt^)  would  then  be  equal  to  the  old  x(t^)  plus  the 
calculated  correction  factor.  Increased  computational  load 
would  only  occur  when  the  residuals  are  outside  the  one-sigma 
bound.  Thus,  with  minimal  calculations  and  additional 
computer  loading,  unmodeled  affects  may  be  compenstated  for 
using  this  ad  hoc  proposal.  Other  ad  hoc  proposals  that  may 
be  beneficial  are  outlined  in  a  paper  by  Maybeck,  Jensen,  and 
Harnly  (24).  In  particular,  this  reference  found  that  using 


[x(t^  )-x(ti")]  to  recalculate  state  estimates  could  serve  as 
a  model  correction  to  account  for  both  filter  bias  and 
maneuver  detection  of  the  target.  Both  the  above  approaches 
should  be  studied  to  determine  which  one  provides  the  most 
performance  enhancement.  Then,  based  on  the  degree  of 
performance  enhancement,  it  should  be  decided  if  the 
additional  states  are  required  (assuming  the  angle  errors  can 
not  be  obtained  from  the  radar  control  unit). 
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LITERATURE  REVIEW  FOR  ACM 


ARTICLES 

Singer,  Robert  A.,  "Estimating  Optimal  Tracking  Filter 
Performance  for  Manned  Maneuvering  Targets,"  IEEE 
Transactions  on  Aerospace  and  Electronic  Systems.  Vol . 


Abstract :  The  majority  of  tactical  weapons  systems 
require  that  manned  maneuverable  vehicles,  such  as 
aircraft,  ships,  and  submarines,  be  tracked  accurately. 
An  optimal  Kalman  filter  has  been  derived  for  this 
purpose  using  a  target  model  that  is  simple  to  implement 
and  that  represents  closely  the  motions  of  maneuvering 
targets.  Using  this  filter,  parametric  tracking 
accuracy  data  have  been  generated  as  a  function  of 
target  maneuver  characteristics,  sensor  observation 
noise,  and  data  rate  and  that  permits  rapid  a  priori 
estimates  of  tracking  performances  to  be  made  when 
maneuvering  targets  are  to  be  tracked  by  sensors 
providing  any  combination  of  range,  bearing,  and 
elevation  measurements. 

Fitts,  John  Murray,  "Aided  Tracking  as  Applied  to  High 
Accuracy  Pointing  Systems,"  IEEE  Transactions  on 
Aerospace  and  Electronic  S  *•**  "  ' 14 ' 


Abstract :  Two  basic  concepts  of  rate  aided  tracking  and 
position  aided  tracking  are  applied  to  a  conventional 
pointing  system  in  order  to  improve  performance.  The 
aided  track  signals  are  derived  in  an  inertial  space 
format  and  are  generated  from  a  Kalman  filter  algorithm. 
Computational  results  are  included  to  show  the  interplay 
between  the  conventional  pointing  system  and  the  aided 
track  filter. 

Pearson,  John  B.,  Edwin  B.  Stear,  "Kalman  Filter 
Applications  in  Airborne  Radar  Tracking,"  IEEE 
Transactions  on  Aerospace  and  Electronic  Systems.  Vol. 


Abstract :  This  paper  studies  the  application  of  Kalman 
filtering  to  single-target  track  systems  in  airborne 
radar.  An  angle  channel  Kalman  filter  is  configured 
which  incorporates  measures  of  range,  range  rate,  and 
on-board  dynamics.  Theoretical  performance  results  are 
given  and  a  discussion  of  methods  for  reducing  the 
complexity  of  the  Kalman  gain  computation  is  presented. 


A  suboptimal  antenna  controller  which  operates  on  the 
outputs  of  the  angle  Kalman  filter  is  also  described. 

In  addition,  methodological  improvements  are  shown  to 
exist  in  the  design  of  range  and  range-rate  trackers 
using  the  Kalman  filter  configuration. 

Farrel,  James  L. ,  Elmen  C.  Quesinberry,  Charles  D. 
Morgan,  and  Michael  Tom.  "Dynamic  Scaling  for 
Air-to-Air  Tracking,"  Proc.  Nat.  Aerospace  and  Electron. 
Conf . ,  Dayton,  Ohio:  157-162  ( May  1975 ) .  • 

Abstract :  This  paper  presents  an  air-to-air  tracking 

approach  providing  accurate  estimates  of  position  and 
velocity  vectors,  plus  target  acceleration  which 
enhances  fire  control  and  track  in  blind  conditions 
(e.g.,  main  beam  clutter)  or  severe  dynamic  conditions 
(e,g.,  close  range,  high-g  maneuvers).  Even  with  a  host 
of  degradations  at  realistic  levels,  accuracies  on  the 
order  of  30  ft,  2  mrad,  and  3  mrad/sec  are  obtained  for 
range,  LOS,  and  LOS  rate,  respectively,  in  steady-state 
track.  Transient  performance  is  characterized  by 
monotonic  reduction  of  lock-on  errors  and  suppression  of 
disturbances.  The  system  integrates  readily  with  all 
INS  and  radar  data,  while  fully  capitalizing  on  all 
processing  performed  with  no  loss  of  efficiency. 

Complete  flexibility  of  mechanization  is  easily 
exploited  to  provide  backup  in  the  event  of  INS  failure. 


Landau,  Mark  I.,  "Radar  Tracking  of  Airborne  Targets," 
Proc.  Nat.  Aerospace  and  Electron.  Conf.,  Dayton,  Ohio: 
(May  1976). 

Abstract :  Airborne  radar  target  trackers  are  required 
to  provide  accurate  estimates  of  target  motion  for  radar 
and  fire  control  functions.  Precise  target  tracking 
accuracies  can  be  achieved  through  the  use  of  modern 
estimation  techniques.  This  paper  examines  a  Kalman 
filtering  approach  to  airborne  tracker  design. 
Alternative  tracking  configurations  for  a  single  target 
track  environment  are  presented,  and  the  characteristics 
of  each  configuration  are  discussed.  Tracking 
performance  of  one  of  the  configurations  is  also 
presented  to  illustrate  typical  target  tracking 
accuracies . 

Tenney,  Robert  R.,  Ralph  S.  Hebbert,  and  Nil  R.  Sandeli, 
"A  Tracking  Filter  for  Maneuvering  Sources,"  IEEE 
Transactions  on  Automatic  Control:  246-251  (April  1977). 

Abstract :  It  is  well  known  that  the  extended  Kalman 
filtering  methodology  works  well  in  situations 
characterized  by  a  high  signal-to-noise  ratio,  good 
observability  and  a  valid  state  trajectory  for 


linearization.  This  paper  considers  a  problem  not 
characterized  by  these  favorable  conditions.  A  large 
number  of  ad  hoc  modifications  are  require  to  prevent 
divergence,  resulting  in  a  rather  complex  filter. 
However,  performance  is  quite  good  as  judged  by 
comparison  of  Monte  Carlo  simulations  with  the 
Cramer-Rao  lower  bound,  and  by  the  filters  ability  to 
track  maneuvering  targets. 

7.  Gholson,  Norman  H.,  Richard  L.  Moose,  "Maneuvering 

Target  Tracking  Using  Adaptive  State  Estimation,"  IEEE 
Transactions  on  Aerospace  and  Electronic  Systems,  VoTT 

AES-13 ' 73 ) :  310-317  (fray  19777. - - - 

Abstract :  Two  approaches  to  a  nonlinear  state 

estimation  are  presented.  The  particular  problem 
addressed  is  that  of  tracking  a  maneuvering  target  in 
three-dimensional  space  using  spherical  observations 
(radar  data).  Both  approaches  rely  on  semi-Markov 
modeling  of  target  maneuvers  and  result  in  effective 
algorithms  that  prevent  the  loss  of  track  that  often 
occurs  when  a  target  makes  a  sudden,  radical  change  in 
its  trajectory.  Both  techniques  are  compared  using  real 
and  simulated  radar  measurements  with  emphasis  on 
performance  and  computational  burden. 

8.  Moose,  R.L.,  H.F.  Vanlandingham ,  and  D.H.  McCabe, 
"Modeling  and  Estimation  for  Tracking  Maneuvering 
Targets,"  IEEE  Transaction  on  Aerospace  and  Electronic 
Systems,  Vol.  AES-15  (3);  448-456  (May  1979). 

Abstract :  A  new  approach  to  the  three-dimensional 

airborne  maneuvering  target  tracking  problem  is 
presented.  The  method,  which  combines  the  correlated 
acceleration  target  model  of  Singer  (3)  with  the 
adaptive  semi-Markov  maneuver  model  of  Gholson  and  Moose 
(8),  leads  to  a  practical  real-time  tracking  algorithm 
that  can  be  easily  implemented  on  a  modern  fire-control 
computer.  Preliminary  testing  with  actual  radar 
measurements  indicates  both  improved  tracking  accuracy 
and  increased  filter  stability  in  response  to  rapid 
target  accelerations  in  elevation,  bearing  and  range. 

9.  Maybeck,  Peter  S., William  H.  Worsley  and  Patrick  M. 

Flynn.  "Investigation  of  Constant  Turn-Rate  Dynamics 
Models  in  Filters  for  Airborne  Vehicle  Tracking,"  Proc. 
Nat.  Aerospace  and  Electron.  Conf . ,  Dayton,  Ohio: 

8?6-9or(F£y'iM2r. -  - — — - 

Abstract:  A  constant  turn-rate  model  for  acceleration 
has  been  proposed  as  more  representative  of  airborne 
vehicle  motion  characteristics  than  models  currently 
employed,  such  as  first  order  Gauss-Markov  or  Brownian 
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motion.  Target  trackers  in  the  form  of  extended  Kalman 
filters  based  on  these  alternative  dynamics  models  are 
developed  and  analyzed  for  both  air-to-air  gunnery 
(estimation  in  three  dimensions)  and  FLIR  image  focal 
plane  target  intensity  tracking  (in  two  dimensions).  In 
both  applications,  the  filter  based  on  the  constant 
turn-rate  model  displays  smalller  estimation  biases, 
indicative  of  a  better  internal  model  within  the  filter 
structure,  but  a  moderate  performance  enhancement  is 
offset  by  a  significant  increase  in  computational 
loading . 

Asseo,  Sabi  J.  and  Richard  J.  Ardila. 

"Sensor-Independent  Target  State  Estimator  Design  and 
Evaluation,"  Proc.  Nat.  Aerospace  and  Electron.  Conf., 
Dayton,  Ohio:  916-924  (May  1982]. 

Abstract :  ...develop  an  estimator  whose  structure  is 

invariant  with  sensor  configuration  and  adaptable  to 

various  target  maneuvers .  Two  alternate  estimator 

designs  are  developed  herein  by  using  local-level 
inertial  (Cartesian)  coordinates  and  line-of-sight 
(spherical)  coordinates.  In  each  estimator,  target 
accelerations  are  represented  as  a 
second-order-dependent  coefficients  which  adapt 
automatically  to  target  maneuvers.  The  performance  of 
these  estimates  is  evaluated  in  simulated  air  combat. 
Both  estimators  produce  unbiased  estimates  of  target 
acceleration,  more  accurate  velocity  and  range 
estimates,  and  a  smaller  miss  distance  than  a  typical 
state-of-the-art  estimator. 

Scharf,  Louis  L.,  Sigurdur  Sigurdsson,  "Fixed  Point 
Implementation  of  Fast  Kalman  Algorithms,"  Report  to 
Office  of  Naval  Research  under  contract 
N00014-82-K-0300,  Arlington,  VA  (November  1983). 

Abstract :  In  this  paper  we  study  scaling  rules  and 

round-off  noise  variances  in  a  fixed  point 
implementation  of  the  Kalman  predictor  for  an  ARMA  time 
series  observed  noise-free.  The  Kalman  predictor  is 
realized  in  a  fast  form  that  uses  the  so-called  fast 
Kalman  gain  algorithm.  The  algorithm  for  the  gain  is 
fixed  point. 

Scaling  rules  and  expressions  for  rounding  error 
variances  are  derived.  The  numerical  results  show  that 
the  fixed  point  realization  performs  very  closely  to  the 
floating  point  realization  for  relatively  low-order  ARMA 
time  series  that  are  not  too  narrowband. 

The  predictor  has  been  implemented  in  16-bit  fixed 
point  arithmetic  on  an  INTEL  8086  microprocessor,  and  in 
16-bit  floating  point  arithmetic  on  an  INTEL  8080. 

Fixed  point  code  was  written  in  ASSEMBLY  language  and 


~rr>yM  vj-.v 


floating  point  code  was  written  in  FORTRAN. 

Experimental  results  were  obtained  by  running  the  fixed 
and  floating  point  filters  on  identical  data  sets.  All 
experiments  were  carried  out  on  an  INTEL  MDS  230 
Development  System. 

Chang,  Chaw-Bing  and  John  Tabaczynski.  "Application  of 
State  Estimation  to  Target  Tracking,"  IEEE  Transactions 
on  Automatic  Control,  Vol.  AC-29  (6):  98-109  (February 

vmv. - 

Abstract :  In  this  paper,  we  present  a  survey  of 

problems  and  solutions  in  the  area  of  target  tracking. 
The  discussion  includes  design  tradeoffs,  performance 
evaluation,  and  current  issues. 


THESES 

Kolibaba,  R.L.,  Precision  Radar  Pointing  and  Tracking 
Using  an  Adaptive  Kalman  Filter.  M.S.  thesis,  Air  Force 
Institute  of  Technology,  Wright-Patterson  AFB, 

Ohio( January  1973). 

Abstract :  An  Adaptive  Extended  Kalman  Filter  technique 
was  developed  to  improve  the  tracking  capabilities  of  an 
aLfborne  tracking  system.  A  maneuver  determination 
technique  was  developed  as  well  as  an  adaptive  technique 
for  the  probability  model  of  target  acceleration.  The 
computer  simulation  was  run  using  four  different  types 
of  data.  Computation  of  the  acceleration  state  was 
inaccurate  due  to  imprecise  modeling  of  the  acceleration 
states.  The  adaptive  technique  reduced  the  range  error 
of  the  non-adaptive  filter.  A  maneuver  was  determined 
when  a  bias  was  detected  on  the  range  measurement 
residual . 


Kalman  Filter(s )  :  Adaptive  and  extended. 
Radar  Model (s):  Not  determined. 


Trajectory  Generation:  Target  vehicle  only,  x-y  plane 
only  (see  page  29). 

Acceleration  Model:  Two  triangular  peaks  (see  page  20). 
DTIC  Number:  AD  768378 

Lindberg,  E.K.,  A  Radar  Error  Model  and  Kalman  Filter 
for  Predicting  Target  States  in  an  Air-to-Air 
Environment.  M.S.  thesis,  Air  Force  Institute  of 


Technology,  Wright-Patterson  AFB,  Ohio ( December , 1974 ) . 


Abstract :  Various  extended  Kalman  filters  and  modified 
nonlinear  filters  are  examined  for  target  acceleration 
estimation  over  a  representative  trajectory.  The 
filters  are  of  two  basic  types.  The  first  type  uses  the 
radar  measurements  in  the  antenna  centerline  coordinate 
frame  to  provide  the  Kalman  filter  measurements.  The 
propagation  equations  for  this  implementation  have 
position,  velocity,  and  acceleration  as  the  states  in  a 
convenient  inertial  reference  frame.  In  comparison,  the 
second  type  performs  propagation  and  measurement 
equations  computations  entirely  in  the  antenna 
centerline  frame. 

Each  type  of  filter  is  tested  for  any  degradation  of 
performance  apparent  from  excluding  radar  measurements 
of  the  error  angles  between  the  antenna  centerline  and 
line-of-sight  reference  frame. 

Due  to  the  simplicity  and  accuracy  of  the  extended 
Kalman  filter  of  the  first  type  that  does  not  include 
measurements  of  the  error  angles,  that  filter  is  then 
chosen  for  further  evaluation.  This  testing  involves 
another  trajectory  and  includes  the  task  of  estimating 
relative  velocity,  position,  and  acceleration  of  the 
target . 

Another  outcome  of  the  thesis  is  a  radar  error  model 
representative  of  the  1960  generation  of  air-to-air 
radars.  This  analytically  useful  result  includes 
appropriate  stochastic  models  of  the  inaccuracies  of  the 
radar,  heretofore  unavailable  in  such  system 
descriptions . 

The  results  demonstrate  that  the  filter  is  extremely 
sensitive  to  certain  maneuvers.  A  graphic  filter 
performance  analysis  is  included  that  displays  this 
sensitivity  and  portrays  overall  filter  performance. 

Kalman  Filter ( s ) :  Extended 

Radar  Model ( s ) :  Azimuth,  elevation  (decoupled 
assumption),  and  range. 

Trajectory  Generation:  Model  by  McDonnell  Aircraft 
Company.  Expo  IV,  Evaluation  of  Fixed  Base  Tracking 
Simulation.  St.  Louis,  Mo.:  16  March  1()73. 

Acceleration  Model:  1st  order  Gauss-Markov  (see  pages 

24-32). - 

DTIC  Number:  AD-A008671 

Lutter,  R.N.,  Application  of  an  Extended  Kalman  Filter 
to  an  Advanced  Fire  Control  System.  M.S.  thesis,  Air 
Force  Institute  of  Technology,  Wright-Patterson  AFB, 


Ohio  ( December , 1976  )  . 


Abstract :  An  extended  Kalman  Filter  is  developed  to  aid 
the  tracking  of  an  air-to-air  missile  from  a  maneuvering 
target  aircraft.  The  filter  exploits  knowledge  of  the 
dominant  dynamic  effects  acting  on  a  missile  that  is 
non-thrusting  and  utilizing  a  proportional  navigation 
guidance  scheme,  i.e.  accelerations  due  to  aerodynamic 
forces.  It  is  designed  to  provide  both  dynamic  tracking 
estimates  in  a  local  inertial  frame  and  estimates  of  the 
proportional  navigation  constant  and  another  pertinent 
parameter . 

A  feasibility  analysis  of  the  filter  is  conducted. 

Its  performance  is  compared  to  a  more  conventional 
filter  that  utilizes  a  first  order  Gauss-Markov  random 
process  acceleration  model.  In  addition,  an  evaluation 
is  made  of  the  filter's  capability  to  recover  from  large 
initial  errors  in  state  estimates. 

The  study  establishes  the  feasibility  of  the 
modelling  approach.  The  estimates  provided  by  the 
designed  filter  are,  in  general,  less  sensitive  to 
system  measurement  noises.  The  filter  performance  is 
trajectory  dependent,  however,  and  the  requirement  for  a 
higher  order  missile  model  within  the  filter  system 
model  is  established  (a  zero-order  model  was  used  to 
develop  as  simple  a  filter  as  would  provide  adequate 
performance ) . 

The  results  of  the  study  strongly  suggest  that  the 
navigation  constant  can  be  estimated  by  the  filter.  The 
recovery  analysis  provides  additional  insights  into  the 
filter's  ability  to  estimate  this  parameter.  It  gives  a 
general  indication  of  the  effects  that  varying  the 
initial  variance  and  noise  strength  (on  the  navigation 
constant  channel)  have  on  the  tuning  and  recovery 
characteristics  of  the  navigation  constant  estimate.  A 
graphic  filter  analysis  is  included  that  portrays  the 
estimation  accuracy  and  recovery  characteristics  of  the 
filter . 

Kalman  Filter(s ) :  extended 

Radar  Model(s):  Gimballed  platform  with  rate  gyros  (see 
pages  9-50). 

Trajectory  Generation;  TRAJ 
Acceleration  Model:  Based  on  dynamics. 

DTIC  Number;  AD  A035293 

Ryan,  J.  E.,  Sensitivity  Study  or  Strapdown  Inertial 
Sensors  in  High  Performance  Applications,  M.S.  thesis, 
Air  Force  Institute  of  Technology,  Wright-Patterson  AFB, 


Ohio  (December  1980) 


Abstract :  This  study  uses  a  computer  simulation  of  a 

strapdown  laser  gyro  inertial  reference  system  to 
analyze  the  errors  generated  as  a  result  of  highly 
dynamic  flight  profiles.  A  stochastic  error  model  using 
state-of-the-art  inertial  sensors  is  developed  in  detail 
and  implemented  in  software.  SOFE,  a  generalized 
simulation  program,  was  used  to  implement  both  a  Monte 
Carlo  simulation  and  a  covariance  analysis.  The  Monte 
Carlo  method  was  selected  to  perform  the  error  analysis. 

Two  highly  dynamic  flight  trajectories  were  developed 
using  the  flight  profile  generator,  PROFGEN.  The 
PROFGEN  program  itself  was  modified  to  include  an 
aircraft  roll  time  constant  and  a  roll-only  maneuver. 

The  errors  generated  in  the  inertial  reference  system  as 
a  result  of  these  flight  trajectories  were  investigated. 
Both  an  error  budget  and  an  analysis  of  the  maneuvers 
inducing  these  errors  were  accomplished. 

Gyro  error  sources  induced  the  most  system  error  and 
coupled  the  dynamics  of  the  flight  trajectory  into  the 
variations  of  the  error.  Misalignment  was  found  to  be 
the  major  cause  of  both  the  accelerometer  and  gyro 
induced  error.  Successive  maneuvers  were  found  that 
reinforced  system  errors  and  other  maneuvers  were  found 
that  cancelled  these  errors.  Also,  some  cases  were 
found  where  the  amount  of  system  error  varied  with  a 
change  in  heading. 

Kalman  Filter ( s ) :  not  determined 

Radar  Model ( s ) :  not  determined 

Trajectory  Generation:  PROFGEN  (modified  for  roll  only) 
Acceleration  Model:  not  determined 
DTIC  Number:  AD  A100825 

Bryan,  R.S.,  Cooperative  Estimation  of  Targets  by 
Multiple  Aircraft.  M.S.  thesis,  Air  Force  Institute  of 
Technology,  Wright-Patter son  AFB,  Ohio  (June  1980). 

Abstract :  (not  copied) 

Kalman  Filter ( s ) :  not  determined 

Radar  Model ( s ) :  Simplified  (see  pages  8-18). 

Trajectory  Generation:  Used,  but  not  included.  Thrust 
and  roll  assumed  to  be  instantly  achieved,  producing 
unrealistic  step  changes  in  target  acceleration  (see 
page  51). 


Acceleration  Model:  First  order  Gauss-Markov  and 
constant  turn  rate  (see  page  8). 

DTIC  Number:  AD  A085799 


Worsley,  W.H.,  Comparison  of  Three  Extended  Kalman 
Filters  for  Air-to-Air  Tracking.  M.S.  thesis,  Air  Force 
Institute  of  Technology,  Wright-Patterson  AFB,  Ohio 
(December  1980). 

Abstract :  The  performances  of  the  extended  Kalman 
filter  implementations  for  three  different  target 
acceleration  models  that  estimate  target  position, 
velocity,  and  acceleration  states  for  air-to-air  gunnery 
were  compared.  The  models  included  1)  a  first  order 
zero-mean  Gauss-Markov  relative  target  acceleration 
model,  2)  a  first  order  zero-mean  Gauss-Markov  total 
target  acceleration  model,  and  3)  a  constant  turn  rate 
target  acceleration  model.  Measurements  available  to 
the  extended  Kalman  filter  at  update  were  the  range, 
range  rate,  and  the  error  angles  between  the  true  line 
of  sight  and  the  estimated  line  of  sight.  Additional 
evaluations  of  the  effect  of  variations  in  the  variances 
of  measurement  noises  were  conducted  for  the  extended 
Kalman  filter  using  the  constant  turn  rate  target 
acceleration  model.  All  evaluations  were  accomplished 
using  Monte  Carlo  simulation  techniques. 

Kalman  Filter ( s ) :  extended 

Radar  Model(s):  Tracker  dynamics  not  included  in  the 
filter  model,  assumed  tracker  moves  to  new  position 
without  error  before  next  measurement  (see  pages  24,29). 
Assumed  to  be  inertially  space-stabilized. 

Trajectory  Generation:  TRAJ 

Acceleration  Model:  See  abstract. 

DTIC  Number:  AD  A094767 

Flynn,  P.M.,  Alternative  Dynamics  Models  and  Multiple 
Model  Filtering  for  a  Short  Range  Tracker.  M.S.  thesis, 
Air  Force  Institute  of  Technology,  Wright-Patterson  AFB, 
Ohio  (December  1981) 

Abstract :  The  performance  of  three  extended  Kalman 
filter  implementations  that  estimate  target  position, 
velocity,  and  acceleration  states  for  a  laser  weapon 
system  are  compared  using  various  target  acceleration 
trajectories.  Measurements  available  to  the  extended 
Kalman  filters  each  update  are  taken  directly  form  the 


outputs  of  a  forward  looking  infrared  (FLIR)  sensor. 

Two  dynamics  models  considered  for  incorporation  into 
the  filter  are  1)  a  Brownian  motion  (BM)  acceleration 
and  2)  a  constant  turn  rate  (CTR)  target  dynamics  model. 
The  CTR  filter  was  compared  against  the  BM  filter  to  see 
if  the  more  complex  dynamics  of  the  CTR  filter  gave  it  a 
significant  improvement  in  tracking  performance  over  the 
BM  filter.  These  two  simple  extended  Kalman  filters 
were  then  compared  to  a  multiple  model  adaptive  filter 
consisting  of  a  bank  of  three  filters  based  on  the 
Brownian  motion  acceleration  model.  All  three  filter 
are  tested  using  three  different  flight  trajectory 
simulations:  a  2  g,  a  10  g  and  a  20  g  pull  up  maneuver. 
All  evaluations  are  accomplished  using  Monte  Carlo 
simulation  techniques. 

The  constant  turn  rate  extended  Kalman  filter  was 
found  to  outperform  the  other  two  filters.  The  main 
advantage  this  filter  had  was  the  minimization  of  mean 
bias  error  in  estimating  position.  The  standard 
deviation  of  error  was  also  slightly  lower  in  most 
instances . 

Kalman  Filter(s):  extended 
Radar  Model ( s ) :  not  determined 
Trajectory  Generation:  2,  10,  and  20g  pull  ups. 
Acceleration  Model:  see  abstract 


DTIC  Number:  AD  A115503 


Appendix  B 

Trajectory  Generation  Program 
PROGRAM  HILLS  (Truth  Model) 

C 

C*#*#*#  AP0-120  RADAR  MODEL — RADAR. FOR  ******* 

C  VERSION  1 

C 

C  DESCRIPTION:  THIS  PROGRAM  DRIVES  THE  APG-120  radar  model, 

C  AIRCRAFT/TARGET  GEOMETRY  MODEL  AND  FILTER  GIVEN  INITIAL 

C  CONDITIONS  AND  TIME  STEP  CONDITIONS. 


C  *THIS  PROGRAM  REQUIRES  SUPPORT  ROUTINES  DEFINED  IN  THE  FILES 

C  RADAR. FTN  AND  ROTATE. FOR. 

*****BOTH  RADAR. FTN  AND  ROTATE. FOR  ARE  INCLUDED  IN  THIS  VERSION 
*****OF  PROGRAM  HILLS. 

C  *INPUTS  TO  THIS  PROGRAM  ARE  CONTAINED  IN  A  TEST  DEFINITION 

C  FILE  INTEST  (FORMERLY  TEST 1 . DAT ) 

C 

COMMON/MAIN/TINL , TFNL , NNT , NDIFF , NTSOUT , 

1  NDATA,NOUT 

COMMON/STEP/T , DT , DT2 

COMMGN/RADAR/Y (2,5)  ,Y1<2,5)  ,  Y2 (  2 , 5 )  , XI  ( 2 , 2 ) , OUT ( 2 , 2 ) , 

1  A,B,C,D, E ,GA ,KA,K,C0EF1,CQEF2,CQEF3, C0EF4 , C0EF5 ,  C0EF6 , 

2  XX(3),YY(3), MODE , IRADAR 

***** ADDED  TANG******************************************************* 
COMMON/AIR/RATE, PHI, PHIDT,PPHI, PITCH, PITCHD,PPITCH,TRATE, 

1  TURN ,  TURND ,  F'TURN  ,TR1 ,  TR2 ,  TR3 ,  TR4 ,  TF1 ,  TF2 ,  TF3 ,  TF4 ,  ANGLE  , 

2  ASPECT , RANGE , RDOT , RANGEP , AOA , TAS , TAST , ALPHA , BETA , ALPHA 1 , 

3  ALPBET , BETA1 , XFDIST , YFDIST , XTDIST , YTDIST ,XTCON , YTCON , 

4  HORT 1 , HORT2 , HORTG , TGEES , DELTA , I AIR , TANG 

*****AT,VTI JKO,ATI JKO  ADDED**************************************** 
C0MM0N/FILTER/VT(3) ,VF(3> ,VR(3> , NFIL , IFILT , AZ , EL , UK , UJ , ROLL , 

1  YAW,  YNAZ ,  YNEL ,  YNWN , YNWJ ,  YNROL.L  , YNYAW , FR, SN1 ,  SN2 , SN3 ,  CONL.P1 , 

2  C0NLP2 , F'l ,  1 1 , 12, U1 , U2, VI  ,V2,VT1(3)  ,VT2(3> , 

3  AT(3) ,VTIJK0(3) ,ATI JK0(3) 

C0MM0N/RK4SAV/SAV (2,5) 

LOGICAL  NFIL 

REAL  K , KA 

C  DIMENSION  NAME (30) 

C  BYTE  NRESP 

**#**THE  FOLLOWING  2  LINES  ARE  USED  IN  OUTPUTTING  FORM  3,  ADDED  CODE*** 
*#*##**#DIMENSION  XSXYZ ( 3 ) , XSTURN( 3 ) , XNED ( 3 ) ,XXYZ ( 3 > ,XI JKO( 3 ) 

DIMENSION  VTI JK ( 3 ) ,  VTIJKAZ(3) 

CHARACTER  TITLE3*40,M0DE3*20,NAME*30 


C 

C 


C 


DATA  Y/10*0./,  Yl/10#0 . / 


URITE(5,?00) 

WRITE(*,900) 


! PROGRAM  BANNER 


C 

C 

C2 

2 

C 


SELECT  OUTPUT  FORMAT  AND  LOAD  OFP  ******* 


WRITE(5,*) 'SELECT 
WRITE(*,*) 'SELECT 
WRITE(5,*) 'FORMAT 
URITE(*,*> 'FORMAT 
WRITE(5,*) 'FORMAT 


OUTPUT  FORMAT  AS  FOLLOWS*.' 

OUTPUT  FORMAT  AS  FOLLOWS:' 

0:  T, ROLL, AZ, EL, WK,WJ, TURND, PHIDT, PHI' 
0:  T, ROLL, AZ, EL, WK,WJ, TURND, PHIDT, PHI' 
15  T, FLAG, VFI, J,K, VTI, J,N, TGEES' 


B-l 


•i  v 


o  ui  o  n 


WRITE(*,*) 'FORMAT  It  T , FLAG , VFI , J , K , VTI , J , K , TGEES ' 

C  WRITE ( 5 , * ) ' FORMAT  2t  FORMAT  0  +  FORMAT  1' 

WRITE (*,*) 'FORMAT  21  FORMAT  0  +  FORMAT  1' 

****#ADDED  OUTPUT  FOR  SOFE****************************************** 

WRITE(*,*) 'FORMAT  3tOUTPUT  FOR  SOFE,  6  MEAS,  TRUTH  STATES,  ETC' 
##***END  OF  ADDED  CODE#**##*##***##**#*######**###**#*#***##***###### 

C  READ ( 5 , * ) NOUT 

READ  <  * , * ) NOUT 

C  IF(NOUT  »LT  *0. OR, NOUT .GT .2>T  HEN 

I F ( NOUT . LT . 0 . OR . NOUT . GT . 3 ) THEN 

WRITE(5,*> 'FORMAT  NUMBER  DOES  NOT  EXIST' 

GO  TO  2 
END  IF 
C 

C******  INPUT  TEST  DEFINITION  FILE  ******* 

OPEN  <  UNIT  =  2 , NAM£= ' TEST  1 . DAT ' , TYPE =' OLD ' , READONLY) 

OPEN<  UNIT=2 , FI LE=' INTEST ' , ST  ATUS= ' OLD ' ) 


c 

c 

READ <2, 910) NAME 

READ <2, 910) NAME 

READ (2,#) MODE 

! TITLE  UP  TO  30  CHARACTERS 

10-ACM,  1-LRI  RADAR  CONSTANTS 

c 

RE AD (2,*) MODE 

READ  <  2 , # ) NDAT  A 

! NOT  USED 

c 

READ ( 2, # ) NDATA 

READ ( 2 , * ) DT 

! TIME  STEP 

c 

READ< 2, # ) DT 

READ ( 2 , # ) NTSOUT 

! NO .  TIME  STEPS/OUTPUT 

c 

READ(2,*)NTS0UT 

READ(2,*)TINL 

! INITIAL  TIME 

c 

READ <  2, * ) TINL 

READ(2,*)TFNL 

! FINAL  TIME 

c 

READ<2,#)TFNL 

RE AD (2,*) RATE 

! ROLL  RATE  (RAD/SEC) 

c 

READ (2,*) RATE 

READ (2,*) ALPHA, BET A 

! EXPONENTIAL  CONSTANTS  FOR  ROLL 

c 

READ  <  2 , # ) ALPHA , BETA 

READ ( 2 , # ) TR1 , TF1 

! FORWARD  ROLL  START, STOP  TIMES 

c 

READ(2,#)TR1,TF1 

READ(2,#)TR2,TF2 

! REVERSE  ROLL  START, STOP  TIMES 

c 

READ  <  2 , # ) TR2 , TF2 

READ (2,*) ANGLE 

'AZIMUTH  ANGLE  TO  TARGET  (RAD) 

c 

READ (2,#) ANGLE 

READ<2,#)W 

! NOT  USED 

c 

READ(2,*)W 

RE AD < 2,*) ASPECT 

! ASPECT  ANGLE  (RAD) 

c 

READ< 2,*) ASPECT 

READ (2,#) RANGE 

! TARGET  RANGE  (FT) 

c 

READ (2,#) RANGE 

RE AD ( 2 , # ) TAS , T  AST 

ITRUE  AIR  SPEED-FIGHTER, TARGET 

c 

RF.AD(2,*)TAS,TAST 

READ<2,#)TRATE 

! TURN  RATE  (NEGATIVE-AUTO) 

c 

READ<2,#)TRATE 

READ ( 2 , # ) TR3 , TF3 

! RIGHT  TURN  START, STOP  TIMES 

READ(2,*>TR3,TF3 

C  READ <  2  ,  * ) TR4 , TF  4  ! LEFT  TURN  ST ART, STOP  TIMES 

READ<2,*)TR4,TF4 

C  READ<2,*)SN1 ,SN2,SN3,FR  (NOISE  FACTORS 

READ(2,*)SN1,SN2,SN3,FR 

C  READ(2,*)IFIL  (NOT  USED 

*****ADDED  CODE  +  +  F  +  +  +  +  +++  +  +  +  +  +  +  +  +  +++  +  +  +  P  +  +  +  +  +  +++  +  +  +  +  +  +  +  +  +  F  +  +  + +•  +  +  +  +  +  +  +  + 

C  IFIL  IS  USED  TO  SELECT  FORMATTED  OR  UNFORMATTED  OUTPUT  FOR  SOFE 

C  IF IL=1  RESULTS  IN  FORMATTED  OUTPUT 

*****END  OF  ADDED  CODE  ***#  **  *  *  ********************************  ******** 
R£AD<2,*>IFIL 

C  READ(2,*)H0RT1 ,H0RT2  (TARGET  HORIZONTAL  TURN  TIMES 

READ(2,*)H0RT1,H0RT2 

C  READ(2,*)H0RTG, DELTA  (TARGET  GEES  AND  EXPONENTIAL 

READ < 2, *)HORTG, DELTA 
C 

C******  INITIALIZATION  ******* 

C 

WRITE < 5 , 940 )  NAME 

IF ( MODE .EQ,0)WRITE<5,*) '  MODE  =  ACM' 

IF (MODE* Eft, 1) WRITE (5,*)'  MODE  =  LRI ' 

C 

C  INTERNAL  MISSION  DATA  GENERATION 

C  CALCULATE  LOOP  CONSTANTS  BASED  ON  TIME  AND  OUTPUT  INDICATORS 

C  AND  PRINT  OUT 

C 

320  WRITE ( 5 , * ) '  ' 

C  T =0 .  (INITIAL  TIME 

T =0  * 

C  DT2=DT/2 »  (HALF  TIME  STEP 

DT2-DT/2  * 

NT=<TFNL-TINL)/<DT*NTS0UT>+1 .0 

C  NNT -TFNL/ ( DT*NTSOUT ) +1 . 0  (TOTAL  NUMBER  OF  LOOPS 

NNT -TFNL/ <  DT*NTSOUT ) +1 . 0 

C  NDIFF=NNT-NT  (NUMBER  OF  LOOPS  BEFORE  FIRST  0! 

NDIFF=NNT-NT 

WRITE (5, 960)  DT , ANGLE , RATE , TRATE , ALPHA , BETA , TR1 , TF1 , TR2 , 

1  TF2 , TR3 , TF3 , TR4 , TF4 , H0RT1 , H0RT2 , HORTG , DELTA 

C 

C  INITIALIZE  AIRCRAFT  MODEL 

C 

IAIR=0 
CALL  AIR 
C 

C  INITIALIZE  RADAR  MODEL 

C 

IRADAR=0 
CALL  RADAR 

WR I TE  <  5 , 980 ) RANGE , ASPECT , T AS , TAST , SN1 , SN2 , SN3 , FR 
C 

C  INITIALIZE  FILTER  ROUTINES 

C 

IFILT=0 
CALL  FILTER 


C******  SET  UP  OUTPUT  FILE  ******* 

C 

CLOSE  <  UNIT=2 ) 

*++++ADDED  CODE+ ++++++++++++++++++++++++++++++++++++++++++++++++ 

C  IF  NE=3 ,  THEN  SOFE  OUTPUT,  OTHERWISE  BASELINE  OUTPUT 

IF  <  NOUT  .NE.  3)  THEN 

*****END  OF  ADDED  CODE++++++++++++++++++++++++++++++++++++++++++ 

C  OPEN  ( UNIT=2 , NAME= ' OUTPUT , DAT ' ,TYPE='NEW' ) 

0PEN(UNIT=2,FILE='0UTDAT' ,  STATUS^ ' NEW ' ) 

WRITE! 2 , 900 ) 

WRITE(2,940)  NAME 

IF ( MODE  »NE.1)WRITE(2,*) '  NODE  =  ACM' 

IF (MODE .EG » 1 ) WRITE (2 , * ) '  MODE  =  LRI ' 

WRITE(2,*)'  ' 

WRITE  <  2 , 960 ) DT , ANGLE , RATE , TRATE , ALPHA , BETA , TR1 , 

1  TF 1 , TR2 , TF2 , TR3 , TF3 , TR4 ,  TF4 , HORT 1 , H0RT2 , HORTG , BELT  A 

WRITE  ( 2 , 980 )  RANGE ,  ASPECT ,  TAS ,  TAST  ,  SN.t ,  SN2 ,  SN3 , FR 
***** ADDED  CODE********************** ********************************* 
END  IF 

C  IF  IFIL  IS  1,  SOFE  OUTPUT  IS  FORMATTED,  OTHERWISE  UNFORMATTED 

IF  (NOUT  . EG »  3  .AND.  IFIL  .EG.  1)  THEN 
OPEN( UNIT=3 , FILE* ' OUTDAT ' , STATUS* ' NEW ' ) 

TITLE3='*****APG~120  RADAR  MODEL*****' 

WRITE(3,*)TITLE3 
WRITE(3, 940) NAME 
IF ( MODE. NE.l) THEN 
M0DE3* ' MODE  =  ACM' 

WRITE ( 3 , * ) M0DE3 
END  IF 

IF (MODE. EG. 1) THEN 
M0DE3* ' MODE  -  LRI' 

WRI TE  <  3 , * ) M0DE3 
END  IF 
END  IF 

IF  (NOUT  .EG.  3  .AND.  IFIL  .NE.  1)  THEN 

OPEN! UNI T=3, FILE* ' OUTDAT  ' ,  STATUS* ' NEW '  ,FORM= '  UNFORMATTED ' ) 
TITLE3='*****APG-120  RADAR  MODEL*****' 

WRITE(3)TITLE3 
WRITE(3)NAME 
IF (MODE. NE.l) THEN 
M0DE3= ' MODE  ■  ACM' 

WRITE(3)M0DE3 

ENDIF 

IF ( MODE .EG . 1 ) THEN 
M0DE3* ' MODE  =  LRI' 

WRITE(3)M0DE3 

ENDIF 

ENDIF 

*****END  OF  ADDED  CODE+++++++++++++++++++++++++++++++++++++++++ 

C 

C  WRITE  HEADINGS  FOR  APPROPRIATE  OUTPUT  FORMAT 


C  GO  TO  (400,410,420) ,N0UT+1 

GO  TO  (400,410,420,490) .NOUT+l 
400  WRITE(5, 1000) 

URITE(2, 1000) 

GO  TO  490 

410  WRITE ( 5 , 1010 ) 

WRITE (2,1010) 

GO  TO  490 

420  WRITE(5, 1020) 

WRITE(2, 1020) 

C 

C****** CALCULATIONS  AND  OUTPUT  LOOP****** 

C 

490  DO  750  L=1 , NNT 

*****ADDED  CODE  TO  INITIALIZE  WN1 , WJ1 ,X1 ,X4,X7************************ 

IF  (L  ,EQ.  1)  THEN 
WK1=WK 
WJ1=WJ 

IF  ( XI ( 2 , 1 )  .LT.  0 . ) THEN 

XS7=:SQRT  (  ( RANGE** 2*  (  T AN(  X 1  ( 2 , 1 )  )  ) **2) /  ( 1  ♦  +  ( TAN( XI  ( 2 , 1 )  )  )  **2 )  ) 
END  IF 

IF  (XI (2, 1 )  .GE.  0.)  THEN 

XS7=~SQRT ( ( RANGE* *2* ( TAN( XI (2,1 ) > )**2)/( 1 . + ( TAN( XI ( 2, 1 ) ) )**2) ) 
ENDIF 

XS1=SQRT ( (RANGE**2  -  XS7**2)/(1.  +  (TAN(X1 ( 1 , 1 ) ) >**2> ) 
XS4=XS1*TAN(X1 ( 1 , 1 ) ) 

ENDIF 

*****END  OF  ADDED  CODE********************************************** 


IF(L.LE.NDIFF)  GO  TO 


DETERMINE  OUTPUT 


INDICATE  ROLL 


NF'HIDT =0 

C  RATE  90  PERCENT 

IF ( ADS (PH IDT ) .GE . . 9*RATE  >  NPHIDT  =  1 

C  GO  TO  (500,510,520) ,N0UT+1 

GO  TO  (500,510,520,530) ,N0UT+1 

500  WRITE ( 5 , 2000 ) T , NPHIDT , AZ , EL , WK , W J , TURND , PHIDT , PHI 

WR ITE ( 2 , 2000 ) T , NPHIDT , AZ , EL , WN , W J , TURND , PHIDT , PH I 
GO  TO  575 

510  WRITE (5, 2010 )T, NPHIDT, VF( 1) ,VF(2) ,VF(3>, 

1  VT(1),VT(2),VT(3),TGEES 

WRITE ( 2, 2010 ) T , NPHIDT ,VF(1),VF(2),VF(3), 

1  YT(1),VT(2),VT(3),TGEES 

GO  TO  575 

520  WRITE (5, 2020 )T,AZ, EL, WK,WJ, TURND, PHIDT, PHI, 

1  VF(1) ,UF(2) ,VF(3) ,VT<1) ,VT(2> ,VT (3) ,TGEES 

WR I TE ( 2 , 2020 ) T , AZ , EL , WK , W J , TURND , PHIDT , PHI , 

1  VF(1) ,VF(2) ,VF(3) ,VT(1) ,VT(2) ,VT(3) ,TGEES 

*****ADDED  CODE*TO*WRITE*SOFE*OUTPUT*TO*A#FILE********************** 
GO  TO  575 

530  IF  (IFILtEQ.l) THEN 

WRITE (3,2030 ) T , RANGE ,RDOT , AZ , EL , WK1 , WJ1 , XS1 , VTI JKO( 1 ) , 

1  ATIJKO(l) ,XS4,YTIJK0(2) ,ATIJK0(2) ,XS7,VTI JK0(3) ,ATIJK0(3> , 

2  VT1(1),VT1(2) ,VT1(3>, TURN, PHI 


A.-???1?? 


CALL  ROT  ATE  (6,  XI  (1,1)  ,OTIJKO,OTI  .JKAZ) 

CALL  ROT  ATE  (5,X1(2,1)  ,  OT.T  JKAZ  ,  OTI JK ) 

WKTRUE~ ( OTI JK ( 2 ) -OF ( 2 ) )/RANGE 
W JTRUE= ( OF ( 3 ) -OTI JK ( 3 ) ) /RANGE 

WRITE(3,*) ' TRUE  AZ  =  ',X1(1,1),'  AZ  =  ',AZ,'  TRUE  EL  =  ',X1(2,1), 
1  '  EL  =  ' ,EL 

URITE(3,*) ' TRUE  AZDOT  =  ' , UKTRUE , '  AZDOT  -  ',WK1, 

1  '  TRUE  ELDOT  =  ',WJTRUE,'  ELDOT  =  '  ,WJ1 
WRITE (3,*) 'ERRORS  IN  DEGREES 1  AZ ,  EL,  AZDOT,  ELDOT' 

RTOD-57. 29577951 

WRITE(3,*)  ( X 1 ( 1 , 1 ) - AZ ) #RTOD , '  ' , ( XI ( 2 , 1 > -EL ) *RTOD , '  ', 

1  (WNTRUE-WN1 )#RTOD, '  ' , ( W JTRUE-WJ1 ) *RTOD 

WRITE ( 3 ,  * ) 

ENDIF 

IF  ( IFIL  .NE.  1)  THEN 

WRITE<3>T, RANGE, RDOT,AZ, EL, WN1 ,WJ1 ,XS1 ,OTIJKO( 1 ) , 

1  AT I JKO ( 1 ) ,XS4,0TI JKO ( 2 ) , ATI JKO ( 2 ) ,XS7,0TIJK0<3) ,ATIJK0(3) , 

2  OT 1(1), OT 1(2), OT 1(3), TURN , PHI 
ENDIF 

*****END  OF  ADDED  CODE************************************************ 

575  DO  700  J=1 ,NTSQUT 

CALL  RADAR 
CALL  FILTER 

*****ADDED  CODE  FOR  SOFE  INPUT************************************* 

C  CALCULATE  OUTPUT  OALUES  FOR  WK1,  WJ1 

WK1=WK 
UJ1=UJ 

C  CALCULATE  POSITION  XS  OALUES  (EL  AND  AZ  ARE  IN  RADAR  REFERENCE) 

C  NO  ROTATIONS  ARE  REQUIRED 

IF  ( XI (2, 1 )  .LT.  0 . ) THEN 

XS7=SQRT ( ( RANGE**2* ( T AN ( X 1 ( 2 , 1 ) ) ) *#2 ) / ( 1 . + ( TAN ( X 1 ( 2 , 1 ) ) ) **2 ) ) 
ENDIF 

IF  ( XI ( 2 , 1 )  .GE.  0 . ) THEN 

XS7--SQRT < ( RANGE**2* ( TAN ( X 1 ( 2 , 1 ) ) ) **2 ) / ( 1 . + ( TAN ( X 1 ( 2 , 1 ) ) ) **2 ) ) 
ENDIF 

XS1=SQRT( <RANGE**2  -  XS7**2)/(1.+  (TAN(X1 ( 1 , 1 ) ) )**2) ) 

ENDIF 

XS4=XS1*TAN(X1 < 1 , 1 >  ) 

*****END  OF  ADDED  CODE************************************************ 

700  CONTINUE 

750  CONTINUE 

800  STOP 

C 

900  FORMAT </,8X, '****»  APQ-120  RADAR  MODEL— RADAR . FOR  ***»*', 

1  / , 14X , ' OERSION  1',//) 

C910  FORMAT <//,30A2> 

910  FORMAT  < // , A ) 

C940  FORMAT (8X,30A2) 

940  FORMAT (8X,A) 

950  F0RMAT<////,7X,F8.5,7X,F7.4,6X,F7.4,9X,F7.4,/f 10X,2F8.5, 10X, 

1  2F8»5,5X,F7*l,/,6X,16Il,7X,F8»l,7X,F7»l,6X,F7*4»/> 

C 

960  FORMATUOX, 'TIME  STEP  =  '  ,F6 . 4 ,5X , '  ANGLE  =  ',F8.5, 

1  5X , ' RATE  =  ' ,F8.5,/,10X, 'TRATE  =  ' ,F8.5,5X, ' ALPHA  =  ',F8.2, 


V.V .  *.rV*z 


>  -■  V  V  v 


2  5X  , ' BETA  =  '  ,F8.2,/, 10X, 'TR1  =  ' , F7 . 3 , 3X , ' TF1  =  ',F7.3,3X, 

3  '  TR2  =  '  ,F7.3,3X,  'TF2  ==  '  ,  F7 . 3 ,  / ,  10X  , '  TR3  =  ',F7.3,3X, 

4  ' TF3  =  '  ,  F7  . 3  , 3X , ' TR4  =  ' , F7 . 3 , 3X , ' TF4  =  ',F7.3,/, 

5  10X,'H0RT1  =  ' , F7 . 3 , 3X , ' H0RT2  =  ' , F7 . 3 , 3X , ' HORTG  =  SF5.2, 

6  3X  , ' DELTA  =  ' ,F8.2> 

980  FORMAT ( 10X , ' RANGE  =  ', F8 . 0 , 3X ,  ' ASPECT  =  ', F8 .5 , 3X, ' TAS  =  ', 

1  F7.1,3X, 'TAST  =  ' , F7 . 1 , / , 10X , ' SN1  =  ' , F6 . 4 , 3X , ' SN2  -  SF6.4, 

2  3X  r ' SN3  =  ' , F6 . 4 , 3X , ' FR  =  ',F5.2> 

990  FORMAT ( 7X , F3 . 5 , 7X , F7 . 4 , 6X , F7 . 4 , 9X  ,  F7 , 4 , / , 1 OX , 2F8 . 5 , 1  OX , 2F8 . 5 , 

1  5X,F7.1 ,/,6X, 1611 ,7X,F8.1 ,7X,F7.1 ,6X,F7,4,/> 

1000  FORMAT </,2X, 'TIME' , IX, ' ROLL S 4X , ' ANTAZ S 5X , ' ANTEL S 7X , ' WK S 

1  8X, 'WJ' , 6X , ' TURND ' ,5X, 'PHIBT' ,5X, 'PHI '  ,/> 

1010  FORMAT </,2X,  'TIME' ,2X, 'ROLL' , 4X , ' VFI ' , 7X , ' OF J ' , 7X , ' VFK ' , 

1  7X , ' VTI ' ,7X, 'VTJ' , 7X , ' VTN ' , 5X , ' TGEES ' ,/) 

1020  FORMAT </,2X, 'TIME' ,5X, 'ANTAZ' ,4X, 'ANTEL' ,6X, 'WK' , 

1  7X,  'UJ'  ,5X,  'TURND'  , 4X , ' F'HIDT '  ,4X,  'PHI '  , 

2  6X , ' VFI ' , 5X , ' OF J ' , 5X , ' VFK ' ,5X, 'VTI ' , 5X , ' VT J ' , 5X , ' VTK ' , 

3  4X, 'TGEES' ,/) 

2000  FORMAT (1X,F6.3,2X,I1 ,7<2X,F8.5) > 

2010  FORMAT ( IX » F6 • 3 , 2X , 1 1 ,6(3X,F7.1 ) ,3X,F5»2) 

2020  F0RMAT(1X,F6.3,2X,7<F8.5,1X),6(F7.1,1X),2X,F5.2) 

*##**ADDED  SOFE  OUTPUT  FORMAT  STATEMENT#####*####*#################### 
2030  FORMAT (F5»2,2X,/,6<F14.6,2X),/,3(F14.6,2X) ,/,3<F14.6,2X>,/, 

1  3(F14.6,2X) ,/,5(F14.6,2X> ) 

END 

C 

C 

SUBROUTINE  FILTER 
C 

c  description:  this  program  calculates  raw  values  of 

C  THE  FIGHTER  AND  TARGET  VELOCITY  FROM  THE  INFORMATION 

C  RECEIVED  FROM  THE  RADAR  AND  AIR  MODELS.  AN  OPTIONAL 

C  NOISE  GENERATOR  AND  LOWPASS  FILTER  IS  INCLUDED. 

**###ADDED  CODE  TO  CALCULATE  TARGET  ACCELERATIONS  IN  VARIOUS  FRAMES#** 
C 

COMMON/MAIN/TINL , TFNL , NNT ,NDIFF ,NTSOUT , 

1  NDATA,NOUT 

COMMON/STEP/T , DT , DT2 

COMMON/RADAR/Y  <  2 , 5 ) , Y1 ( 2 , 5 ) , Y2 ( 2 , 5 ) ,X1 (2,2) , OUT (2, 2) , 

1  A,B,C,D,E,GA,NA,K,C0EF1,C0EF2,CDEF3,C0EF4,C0EF5,C0EF6, 

2  XX(3),YY<3>, MODE , I RADAR 

COMMON/AIR/RATE,  PHI,  F’HIDT,  PPHI,  PITCH,  PITCHD ,  PPITCH ,  TRATE , 

1  TURN, TURND, PTURN, TR1 ,TR2,TR3,TR4,TF1 , TF2 , TF3 , TF4 , ANGLE, 

2  ASPECT ,  RANGE ,  RDOT  ,  RANGEP ,  AOA ,  TAS ,  TAST ,  ALPHA ,  BETA ,  ALF'HAl , 

3  ALPBET ,BETA1 ,XFDIST ,YFDIST ,XTDIST ,YTDIST ,XTCQN, YTCON, 

4  H0RT1 , H0RT2 , HORTG, TGEES , DELTA , IAIR, TANG 
COMMON/FILTER/VT ( 3 ) ,VF<3> ,VR(3),NFIL,IFILT , AZ , EL , WK , W J , ROLL , 

1  YAW , YNAZ , YNEL , YNUK , YNWJ , YNROLL , YNYAW ,FR, SN1 » SN2,SN3,C0NLP1 , 

2  C0NLP2 , PI , 1 1 , 1 2 , U 1 , U2 , V 1 , V2 , VT 1 <  3 ) , VT2 ( 3 ) , 

3  AT(3) ,VTIJK0<3) ,ATIJN0<3) 

C0MM0N/RK4SAV/SAV (2,5) 

*#*##ADDED  DIMENSION  VARIABLES  FOR  LOCAL  ARRAYS***#**##****##**##***** 
DIMENSION  ATXYZ  <3>,ATLMM<3) , ATI JKAZ  <  3 ) , ATNEIK  3 ) , VTNED<  3 )  , 


1 


VTXYZ ( 3 ) 

LOGICAL  NFIL 
REAL  K  f KA 
C 

IF(IFILT.EQ.l  >GO  TO  .10 
C 

C  ##*  FILTER  INITIALIZATION  *** 

C 

C  YNAZ=OUT (1,2)  ! ANTENNA  AZIMUTH  HISTORY 

5  YNAZ=OUT( 1 ,2) 

C  YNEL=OUT (2,2)  ! ANTENNA  ELEVATION  HISTO 

YNEL=0UT(2,2> 

C  YNWK=0UT(1,1>  IRATE  GYRO  AZ  HISTORY 

YNWK=OUT ( 1 r 1 > 

C  YNWJ=0UT(2, 1 >  IRATE  GYRO  EL.  HISTORY 

YNWJ=0UT(2, 1 ) 

C  YNROLL=PHI  I  AIRCRAFT  ROLL  HISTORY 

YNROLL=PHI 

C  YNYAU-  TURN  I  AIRCRAFT  HEADING  HISTOF 

YNYAW=TURN 

C  PI=3.. 1415927  ! DEFINE  PI 

PI=3. 1415926536 

C  C0NLP1=FR*DT/<1.+FR*DT>  I LOWPASS  FILTER  CONSTANT 

CONLPl=FR*DT/( l.+FR*DT> 

C  C0NLP2=1./(1.+FR*DT)  ! LOWPASS  FILTER  CONSTANT 

C0NLP2=1,/(1.+FR*DT) 

C  NFIL= . TRUE ♦  I  NOISE  GENERATOR  FLAG 

NFIL=. FALSE, 

C  11=5  I  RANDOM  NUMBER  SEED 

11=5 

C  12=6  I  RANDOM  NUMBER  SEED 

12=6 

C  IFILT=1  I  FILTER  INITIALIZATION 

IFILT=1 
C 

***** ADDED  CODE  TO  AVOID  NOISE  ADDITION(NOISE  ADDED  IN  SOFE)  ******* 
U1=0. 

U2=0 , 

V1=0, 

V2=0 , 

*****END  OF  ADDED  CODE  ********************************************* 

C  ***  FILTER  PROCESSING  LOOP  *** 

C 

C  NOISE  GENERATOR  TO  ADD  NOISE  TO  AZ,  EL,  WK,  WJ,  ROLL,  AND  YAW 

C  (USES  RANDOM  NUMBER  GENERATOR:  RAN(X, Y) ) 

C 

CIO  NFIL=, NOT. NFIL 

****#ADDED  CODE  TO  FORCE  NFIL=TRUE - AVOIDS  RANDOM  NUMBER  GENERATOR#* 

10  NFIL= • TRUE . 

IF (NFIL)GO  TO  11 

C 

C  U1=SQRT  < -2 . #ALOG (RAN (11,12)))  I  NOISE  GENERATOR 

U1=SQRT ( -2 . *ALOG ( RAN (11,12))  ) 

V1=2.#PI#RAN( 11,12) 


U2=SGRT( ~2.*AL0G(RAN( II , 12) ) ) 
02=2.*PI#RAN(I1  ,12) 

AZ=OUT  (1,2)  +1)1  >KCOS ( VI ) #SN1 
AZ=OUT(  1,2)+U1#C0S(01  )*SN1 
EL=0UT(2,2)+U1*SIN(01 )*SN1 
EL=0UT(2,2)+U1#SIN(01)*SN1 
WK=OUT ( 1 , 1 )+U2#CQS(02)*SN2 
WK=OUT  <  1 , 1 )  +U2*C0S  ( 0  2 )  *SN2 
UJ=0UT(2,1)+U2*SIN(02)*SN2 
W J=OUT (2,1) +U2*SIN ( 02 ) *SN2 
R0LL=PHI+U1*C0S(01 )*SN3 
ROLL=PHI+U1#COS(OD*SN3 
YAW=TURN+U2*C0S(01)*SN3 
YAU=TURN+U2*C0S(01 )*SN3 
GO  TO  12 

AZ=OUT ( 1 ,2>+Ul*SIN(01 )#SN1 
AZ=OUT(  1 ,2)HJ1*SIN(01)#SN1 
EL.=0UT(2,2)+U1*C0S(01  )#SN1 
EL=0UT(2,2)  FU.l#C0S(01  )*SN1 
UK=OUT  (1,1)  +IJ2*SIN  ( 02)  #SN2 
WK=OUT ( 1 , 1 )+U2*SIN(02)*SN2 
W J=OUT (2,1) +U2*C0S ( 02 ) *SN2 
U  J=01JT  (2,1)  +U2#C0S  (  02 )  *SN2 
R0LL=PHI+U1#SIN(01 )*SN3 
ROLL-PHI +U1 #S I N (01 )  #SN3 
YAW=TURN+U2*SIN(01)*SN3 
YAW  =  TURN+IJ2*SIN(01)*SN3 

SINGLE  POLE  LOWPASS  FILTER  ON  DATA 

IF ( FR » EG »0 . )G0  TO  14 
AZ=C0NLP1*AZFC0NLP2*YNAZ 
AZ=C0NLP1#AZ+C0NLP2*YNAZ 
YNAZ=AZ 

EL.  =  C0NLP1*EL+C0NLP2*YNEL 
EL=C0NLP1#EL+C0NLP2*YNEL 
YNEL=EL 

UK=C0NLP1*WK+C0NLF'2*YNWK 

WK=C0NLP1*WK+C0NLP2*YNWK 

YNWK=WK 

WJ=C0NLP1*WJ+C0NLF'2*YNWJ 

UJ=C0NLP1#UJ+C0NLP2*YNWJ 

YNWJ=WJ 

R0LL=C0NLF'1#R0LLFC0NLF'2*YNR0LL 

R0LL=C0NLF'1*R0LL+C0NLP2*YNR0LL 

YNROLl.=RQLL 

YAW=C0NLP1#YAW+C0NLP2*YNYAW 
YAW=C0NLP1*YAUH-C0NLF'2*YNYAW 
YN YAW=YAW 


! ANTENNA  AZIMUTH 


! ANTENNA  ELEOATION 


RATE  GYRO  AZ 


IRATE  GYRO  EL 


! AIRCRAFT  ROLL 


I  AIRCRAFT  HEADING 


! ANTENNA  AZIMUTH 


! ANTENNA  ELEOATION 


IRATE  GYRO  AZ 


IRATE  GYRO  EL 


I  AIRCRAFT  AZ 


! AIRCRAFT  HEADING 


! ANTENNA  AZIMUTH 


! ANTENNA  ELEOATION 


IRATE  GYRO  AZ 


IRATE  GYRO  EL 


I  AIRCRAFT  ROLL 


I  AIRCRAFT  PITCH 


C*****OT 

C 

14 


GENERATE  OELOCITIES  IN  ANTENNA  COORDINATES 

1  IS  FIGHTER  OELOCITY  IN  RADAR  REFERENCE  ( 10 , JO , KO ) #***#*#**#* 
0T1 ( 1 ) =TAS 


.*•  y  -V-  V>  V  • 


VT1 ( 3 ) =0  * 

C*****VF  IS  FIGHTER  VELOCITY  IN  LINE -OF -SIGHT 
CALL  ROTATE (  6  *  AZ  *  VT.l  ,  VT2) 

C  CALL  ROTATE ( 5 , EL , VT2 , VF )  ! FIGHTER  VELOCITY 

CALL  R0TATE(5,EL,VT2,VF> 

VR ( 1 ) =— ROOT 

VR(2>=-RANGE*WK 

VR(3)=RANGE*WJ 

C  VT  <  1 ) =VF ( 1 > -VR ( 1 )  (TARGET  VELOCITY 

C*****VT  IS  TARGET  VELOCITY  IN  LINE-OF-SIGHT 
VT( 1 )=VF< 1 )-VR( 1 ) 

VT  <  2 ) =VF ( 2 ) -VR ( 2  > 

VT ( 3 ) -VF ( 3 ) -VR ( 3 ) 

#*****THE  FOLLOWING  HAS  BEEN  ADDED  FOR  SOFE  INF'UT^^**#***#**^#******* 

C  TARGET  VELOCITY  IN  N-E  PLANE 

VTNED( 1 )=XTC0N/DT2 
VTNEB<  2 ) =YTC0N/DT2 
VTNED ( 3 ) =0 . 

C  ROTATE  FROM  NED  TO  IJKO  FRAME  (PITCH-0.) 

CALL  ROTATE ( 6 , TURN , VTNED , VTX YZ ) 

CALL  R0TATE(4,PHI,VTXYZ,VTIJK0> 

C  TARGET  ACCELERATION  IN  N-E  PLANE 

ATNED(  l  )=TGEES*C0S(PI--TANG>*32.2 
ATNED(2)=T6EES*SIN(PI-TANG>*32.2 
ATNED ( 3  > -0 » 

C  ROTATE  FROM  NED  TO  IJKO  FRAME  (PITCH  ANGLE  =  0.) 

CALL  ROTATE ( 6 , TURN , ATNED , ATXYZ ) 

CALL  ROTATE  ( 4 , PHI , ATXYZ , ATI JKO ) 

C  ROTATE  FROM  IO,JO,KO  TO  IJK  TO  GET  AT ( 1 > , AT ( 2 ) , AT ( 3 ) 

CALL  ROTATE  ( A , AZ , AT I JKO , AT  I JKAZ ) 

CALL  ROTATE  ( 5  » EL  ?  ATI JKAZ  » AT ) 

**###END  OF  ADDED  CODE*#**###*#*##*###*#*####*###**###########*#####*# 
300  RETURN 

END 

SUBROUTINE  ROT  ATE  <  N » ANG  fX,Y) 

C 

C  TITLE 

c  subroutine:  fn:  rotate 

c 

C  VERSION 
C  V-CC-001 

c 

C  AUTHOR  AND  DATE 
C  DESIGNER  —  R.  BEAL 

C  CODER  —  R,  BEAL  JULY  1980 

C 

C  MODIFICATIONS 

C  SPR-NNNACSXXX  VERSION  *NN  DD-MMM-YY  NAME 

C 

C  DESCRIPTION 

C 

C  FUNCTION 

C  THIS  SUBROUTINE  PERFORMS  GENERAL  AIRCRAFT  COORDINATE  SYSTEM 


C  VECTOR  ROTATION. 

C 

C  LOCAL  DATA 
C  T 

C  TEMPORARY  MATRIX  FOR  ROTATION  VALUES. 

C  CAN 

C  COSINE  OF  ROTATION  ANGLE. 

C  SAN 

C  SINE  OF  ROTATION  ANGLE. 

C 

C  CALLING  SEQUENCE  AND  CONDITIONS 

C  THIS  SUBROUTINE  CAN  BE  CALLED  BY  ANY  PROGRAM.  THE  CALL  STRING 

c  is  as  follows: 

C  N  INDEX  FOR  ROTATION  TYPE 

C  ANG  -  ANGLE  FOR  ROTATION  (MUST  BE  IN  RADIANS). 

C  X  INPUT  VECTOR. 

C  Y  -  OUTPUT  VECTOR, 

C 

C  SUBROUTINE/FUNCTION  SUBPROGRAMS 
C  NONE 

C 

C  COMMENTS 
C  NONE 

C 

C  LOCAL  DECLARATION  ST ATEMENT ( 3 > 

C 

DIMENSION  T(3,3) ,X(3) ,Y(3) 

C 

CAN=COS  <  ANG ) 

SAN =SIN (ANG) 

C 

C  GO  TO  (10,20,30,40,50,60),  N  !SET  UP  THE  ROTATION  MATRIX 

GO  TO  (10,20,30,40,50,60),  N 

C  N  =  1 

C  PHI  (ROLL  ANGLE)  -  INVERSE 

10  T(l,l)=l. 

T( 1 ,2>=0. 

T ( 1 , 3 ) =0 . 

T(2, 1 >=0 . 

T ( 2 , 2 ) -CAN 
T(2,3)=-SAN 
T ( 3 , 1 ) =0 . 

T(3,2)=SAN 
T ( 3 , 3 ) =CAN 
GO  TO  70 


C 

c 

20 


N  =  2 

THETA  (PITCH  ANGLE),  LAMBDA  EL,  -2  DEGREES,  ALPHA  -  INVERSE 
T ( 1 , 1 ) =CAN 
T  ( 1 , 2 ) =0 . 

T ( 1 ,3)=SAN 
T ( 2 , 1 ) =0 . 

T (2,2)=1 ♦ 

T ( 2 , 3 ) =0 . 

T (3, 1 )=-SAN 


I(3,2)=0, 

T  (  3 » 3 )  =l'AN 
GO  TO  70 
N  =  3 

LAMBDA  AZ,  PSI  (HEADING  ANGLE)  -  INVERSE 
T  ( 1 , 1 ) =  CAN 
T ( 1 , 2) =  -SAN 
T( 1 ,3)=0. 

T<2, 1 )=SAN 
T(2,2)=CAN 
T(2,3)=0. 

T(3,l)=0. 

T(3,2)=0, 

T ( 3 , 3 ) =1 . 

GO  TO  70 

N  =  A 

PHI  (ROLL  ANGLE) 

T(l,l)=l. 

T(l,2)=0. 

T  ( 1 , 3 ) =0 . 

T  ( 2 , 1 ) =0 . 

T(2,2)=CAN 
T(2,3)=SAN 
T ( 3 , 1 ) =0 . 

T(3,2)=-SAN 
T(3,3)=CAN 
GO  TO  70 
N  =  5 

THETA  (PITCH  ANGLE),  LAMBDA  EL,  -2  DEGREES,  ALPHA 
T ( 1 , 1 ) =CAN 
T ( 1 , 2 ) =0 ♦ 

T(1,3)=-SAN 
T ( 2 , 1 ) =0  * 

T ( 2, 2 ) -1 . 

T ( 2 , 3 ) =0 . 

T ( 3, 1 ) =SAN 
T ( 3 , 2 ) -0 . 

T ( 3 , 3 ) =CAN 
GO  TO  70 
N  =  6 

LAMBDA  AZ,  PSI  (HEADING  ANGLE) 

T ( 1 , 1 ) =CAN 
T ( 1 , 2 ) =SAN 
T ( 1 ,3)=0  * 

T ( 2, 1 ) =  -SAN 
T ( 2, 2 ) =CAN 
T ( 2, 3 ) =0  . 

T  <  3 , 1 ) =0 . 


T ( 3 , 2) =0 . 

T  <3,3>  =  1 ♦ 

PERFORM  THE  ROTATION 

Y(1)=T(1,1>#X(1)+T<1,2)#X(2)+T(1,3)#X(3) 

Y(2)=T(2,1)*X<1)+T(2,2)*X<2>+T(2,3>*X(3) 


V 


c 

c ****** 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


Y(3>=T(3,1)*X<1>+T<3,2>*X<2>+T<3,3)*X<3> 

RETURN 

END 

APG-120  RADAR  MODEL— RADAR . FTN  ******* 

VERSION  1 

DESCRIPTION:  THESE  SUBROUTINES  REPRESENTS  A  COMMON  SET 

FOR  RADAR. FTN  PROGRAMS.  INCLUDED  ARE  THE  APG-120  RADAR 
MODEL,  AN  AIRCRAFT/TARGET  GEOMETRY  MODEL  AND  A  SUPPORTING 
FOURTH  ORDER  RUNGE-KUTTA  ROUTINE. 


SUBROUTINE  RADAR 


RADAR  MODEL  ******* 

DESCRIPTION:  THIS  SUBPROGRAM  MODELS  THE  APG-120  RADAR  FOR  BOTH 

ACM  AND  LRI  MODES.  RADAR  CONTAINS  AUTOMATIC  GAIN  CONSTANT 
CALCULATION  AND  BORESIGHT  ROTATION. 

COMMON/STEP/T , DT , DT2 

COMMON/RADAR/Y (2,5) ,Y1 (2,5) , Y2<  2 , 5 ) , XI (2,2) , OUT (2, 2), 

A , B , C , D , E ,GA , KA , K , COEF1 ,C0EF2 , C0EF3 ,C0EF4,C0EF5, C0EF6 , 

XX ( 3 ) , YY  <  3 ) , MODE , I RADAR 

COMMON/AIR/RATE,  PHI,  PHIDT,PPHI,  PITCH,  PITCHD,PF'ITCH,TRATE, 

TURN, TURND, FTURN, TR1 , TR2 , TR3 , TR4 , TF1 , TF2 , TF3 , TF4 , ANGLE, 
ASPECT , RANGE , RDOT , RANGER , AOA , TAS , T AST , ALPHA , BET A , ALPHA 1 , 
ALPBET ,BETA1,XFDIST,YFDIST ,XTDIST ,YTDIST , XTCON , YTCON , 

HORT1 , HORT2 , HORTG , TGEES , DELTA , I AIR , TANG 
C0MM0N/RN4SAV/SAV (2,5) 

REAL  K ,KA 

IF ( I RADAR . EG ♦ 1 ) GO  TO  70 
INITIALIZE  RADAR  MODEL 

SET  SERVO  CONSTANTS  FOR  SELECTED  RADAR  MODE 

IF ( MODE . EG • 1 )GO  TO  20 

A= . 4995 

B= .0495 

GA=17 » 47 

GO  TO  30 

A=2.008 

B=. 1990 

GA=1 . 092 

C= , 0576 

D= . 2576 

E=2.0 

KA=61 .31 

K=2. 164 

SET  UP  COEFICIENTS  FOR  ANTENNA  MODEL  RUNGE  KUTTA  APPROX. 


B-13 


m  *  %  ’’v’A*" 


*i+±*&r* 


Sw»5 


mm 


C0EF1=-1./B 
C0EF2=KA/D 
C0EF3=KA*C/D 
COEF  4=  — 1 . /D 
C0EF5=K/2./B 
CQEFd>=K/2 . #A/B 
DO  40  1=1,5 
Y  ( 1  ,  I ) =0  » 

Y ( 2 , I ) =0 ♦ 

Y 1 ( 1 , 1 ) =0  » 

Y1 ( 2 , 1 ) =0 . 

CONTINUE 

Y(1,3)=ANGLE 

Y  ( 1 , 3 ) =ANGLE 
Y ( 2 ,3) =0 . 

Y  (  2 , 3 ) =0 . 

XI < 1 , 1 )  =  <  ANGLE-TURN ) #COS ( PHI ) 

X 1 ( 1 , 1 )  =  ( ANGLE-TURN ) *COS  <  PHI ) 

XI (2,1 )=( ANGLE-TURN )*SIN( PHI ) 

X 1 ( 2 , 1 )  =  <  ANGLE-TURN ) *S I N ( PHI > 

XI < 1 ,2)=0. 

XI ( 1 , 2 ) =0 . 

Xl(2,2)=0. 

XI ( 2 , 2) =0  * 

0UT(1,1)=Y(1,4)*C0S(Y(2,3))+X1(1,2> 
OUT  <1,1)=Y(1,4) *COS  (Y(2,3))+X1(1,2) 
OUT (2,1)=Y(2,4)+X1(2,2) 
0UT(2,1)=Y(2,4)+X1 (2,2) 

OUT (1,2)=Y(1,3) 

OUT (1,2)=Y(1,3) 

OUT (2,2)=Y(2,3) 


! ANTENNA  AZ  (RAID 


! ANTENNA  EL  (RAD) 


! AZIMUTH  INPUT  (RAD) 


! ELEVATION  INPUT  (RAD) 


! AZ  RATE  INPUT  (RAD/SEC) 


! EL  RATE  INPUT  (RAD/SEC) 


IRATE  GYRO  AZ  (RAD/SEC) 


IRATE  GYRO  EL  (RAD/SEC) 


I  ANTENNA  AZ  OUTPUT  (RAD) 


I  ANTENNA  EL  OUTPUT  (RAD) 


I RADAR® 1 
I RADAR® 1 
60  TO  90 


I  RADAR  MODEL  INITIALIZED 


RADAR  MODEL  PROCESSING 


DO  85  N® 1,4 
DO  80  I®1,2 
F  =  l. 

IF ( I ♦ EQ  1 1 ) F=COS( OUT (2,2) ) 

Y1 ( I , 1 >®Y< I ,2) 

Y1(I,2)=C0EF1*Y(I,2)  +  AGC1#GA#X1 (1,1)  - 
AGC1*GA#Y(I,3) 

Y1(I,3)=Y(I,4) 

Y1 ( 1 , 4 >=C0EF2#Y (1,5)  +  C0EF3*(C0EF4*Y( I ,5)  - 


E*F*Y(I,4)  +  C0EF5*Y(I,1>  +  C0EF4*Y(I,2)  - 
E*X1(I,2) ) 

Y1 ( 1 , 5) =COEF 4#Y (1,5)  -  E*F*Y(I,4>  +  C0EF5*Y(I,1> 
C0EF6*Y(I,2>  -  E#X1 < I , 2) 

CALL  RK4 (N,5,T,I) 

IF ( N  *  EQ , 2 ♦ OR ♦ N ,EQ  *  4 ) GO  I  l  SO 


i; 


-  v-V 


I F  < I . EQ  *  2 ) GO  TO  73 

T=T-DT2 

GO  TO  80 


C 

C  GENERATE  RADAR  MODEL  INPUTS 

C 

C  73  IF ( I .EQ .2 ) CALL  AIR  (UPDATE  AIR  MODEL 

73  IF ( I . EQ . 2 >CALL  AIR 

XI  (1 , 1 )  =  < ANGLE-TURN) *COS (PHI ) 

XI <2, 1 )=< ANGLE-TURN >*SIN< PHI ) 
XX(1)=F*HIDT-TURND#SIN(PITCH) 

XX ( 2 ) =PI TCHD*COS (PHI) +TURND*COS  <  P I TCH  >  *S IN ( PHI ) 

XX  ( 3 )  =— PI  TCHMSIN  ( PH  I )  +TURND*COS  <  PITCH )  *COS  ( PHI ) 

CALL  R0TATE(5,-. 0349066, XX, YY) 

CALL  R0TATE(6,Y (1,3),YY,XX) 

CALL  R0TATE(5,Y(2,3) ,XX,YY) 

XI ( 1 , 2  >=YY ( 3 ) 

XI (2,2)=YY(2) 

80  CONTINUE 

85  CONTINUE 

C  OUT (1,1)=Y(1,4)#C0S(Y(2,3))+X1(1,2)  (RATE  GYRO  AZ 

0UT(1,1)=Y< 1,4>*C0S(Y(2,3))+X1(1,2) 

C  OUT (2,1)=Y(2,4)+X1(2»2)  (RATE  GYRO  EL 

OUT  (2,1)=Y(2,4)TX1(2,2) 

C  86  OUT (1,2)=Y(1,3)  (ANTENNA  AZ  OUTPUT 

86  OUT  (1,2)=Y(1,3) 

C  OUT <2»2)=Y<2,3)  (ANTENNA  EL  OUTPUT 

OUT (2,2)=Y<2»3) 

C 

C  AUTOMATIC  GAIN  CONSTANT  CALCULATION 

C 

90  B2= ( XI < 1 , 1 ) -OUT (1,2)) #*2 . + ( X 1 ( 2 , 1 ) -OUT (2,2)) **2 . 

GAMM=SQRT ( B2 ) 

IF ( GAMM . GE ♦ . 34 ) GO  TO  120 

GAMM=GAMM~. 02625 

IF ( GAMM . LT . 0 . ) GO  TO  110 

BTH=27.*GAMM 

G2=SIN<  BTH ) /BTH 

G2=G2*G2/( 1 .+.71*BTH«BTH) 

G2=G2*G2 
GO  TO  150 
110  G2-1. 

GO  TO  150 
120  G2=0, 

150  S=1 » 278419E9  #  G2/ < RANGE*RANGE*RANGE*RANGE) 

AGC1=S/ ( S+6 .31E-14) 

C 

RETURN 

END 

C 

C 

SUBROUTINE  AIR 
C 

C*##***  AIRCRAFT  MODEL  ***#### 


noon 


c  description:  this  subprogram  generates  a  simulation  of 

C  THE  AIRCRAFT  AND  TARGET  IN  FLIGHT. 

c 

COMMON/STEP/T , DT , DT2 

COMMON/RADAR/Y (2,5) ,Y1 (2,5) , Y2(2,5) ,X1 (2,2) , OUT <2, 2) , 

1  A ,  B ,  C ,  D ,  E  ,  GA , KA ,  K  ,  COEF 1 ,  COEF2 ,  C0EF3 ,  COEF4  ,  C0F.F5 ,  C0EF6 , 

2  XX ( 3  > , YY ( 3 ) , MODE , I RADAR 

COMMON/ AIR/RATE, PHI , PHIDT , PPHI , PITCH , PITCHD ,PPITCH , TRATE , 

1  TURN , TURND , PTURN , TR1 , TR2 , TR3 , TR4 , TF1 , TF2 , TF3 , TF4 , ANGLE , 

2  ASPECT , RANGE , ROOT , RANGEP , AOA , TAS , T AST , ALPHA , BETA , ALPHA 1 , 

3  ALPBET , BETA 1 , XFD 1ST ,YFD 1ST , XTD 1ST , YTD 1ST , XTCON , YTCON , 

4  HORT1 ,HQRT2,H0RTG, TGEES, DELTA, IAIR, TANG 
COMMON/RK4SAV/SAV (2,5) 

REAL  K , KA 
C 

IF ( I AIR . EG . 1 ) GO  TO  20 
INITIALIZE  AIRCRAFT  MODEL 

PHI =0  »  'ROLL  ANGLE (RAD) 

PH 1=0. 

C  PHIDT=0.  IROLL  RATE ( RAD/SEC ) 

PHIDT---0. 

C  PPHI=0 .  ! PREVIOUS  ROLL  ANGLE 

PPHI=0. 

C  TlJRN=0  *  !  YAW  ANGLE  (RAD) 

TURN=0 . 

C  TURND-O.  ! Y AW  RATE ( RAD/SEC ) 

TURND=0 . 

C  PTURN-O,  !  PREVIOUS  YAL)  ANGLE 

FTURN=0. 

C  PITCH=0 ♦  ! AIRCRAFT  PITCH  (RAD) 

PI TCH=0 . 

C  PITCHD-O.  ! PITCH  RATE  (RAD/SEC) 

PITCHD-O, 

C  PPITCH=PITCH  ! PREVIOUS  PITCH  ANGLE 

F*PITCH=PITCH 

C  A0A=0.  ! ANGLE  OF  ATTACK  (RAD) 

A0A=0 . 

*****  ADDED  CODE  FOR  PSEUDO  INITIALIZATION  ***************** 

F'HIDT1=0 . 

PH IDT 2=0 . 

PHI 1=0 . 

PHI2=0 . 

TURND 1=0. 

TURND2=0 . 

TURN 1=0. 

TURN2=0 . 

TANG=0 , 

****#END  OF  PSEUDO  INITIALIZATION************************************* 
C 

C  CALCULATE  ROLL.  AND  TURN  CONSTANTS 


ALPHA 1=1 ./ALPHA 
ALPBET =ALPHA+BET  A 
BETA1=1 ./ALPBET 

SET  UP  GEOMETRY  (X-AXIS  IS  IN  0- YAW  DIRECTION) 

XFDIST =0 .  ! FIGHTER  X  LOCATION  (FT) 

XFDIST =0 . 

YFDIST=0 »  ! FIGHTER  Y  LOCATION  (FT) 

YFDIST=0. 

XTDIST  =RANGE*COS ( ANGLE )  ! TARGET  X  LOCATION  (FT) 

XTDIST=RANGE*COS ( ANGLE ) 

YTDIST =RANGE*SIN( ANGLE )  ! TARGET  Y  LOCATION  (FT) 

YTDIST=RANGE*SIN( ANGLE) 

RDOT=-(TAST*COS(ASPECT)+TAS*COS(ANGLE) >  ! INITIAL  RANGE  RATE  (FT/SE 
RDOT=- ( TAST*COS ( ASPECT ) +T AS*COS ( ANGLE ) ) 

RANGEP=RANGE  ! PREVIOUS  RANGE  VALUE  (FT) 

RANGEP=RANGE 

XTCON=COS ( ANGLE-3 . 1 41593+ASPECT ) *TAST* 

DT2  !  TARGET  X  VELOCITY  (FT/SEC 

XTCON=COS(  ANGLE-3. 141593+ASF'ECT  )*TAST*DT2 
YTCON=SIN ( ANGLE-3 . 141593+ASPECT ) *TAST* 

DT2  !  TARGET  Y  VELOCITY  (FT/SF.C 

YTCON=SIN ( ANGLE-3 . 1 41593+ASPECT ) *TAST*DT2 
TGEES=0 • 

IAIR=1 
GO  TO  100 

AIRCRAFT  MODEL  PROCESSING 
GENERATE  ROLL  INPUTS 

IF (  T ,  GE  .  TR.1  )PHIDTl  =  RATE/(  AL.PHA1-BETA1 ) *( -EXP 
( -ALPHA* ( T-TR1 ) ) *ALPHA1 +EXP ( -ALPBET* ( T-TR1 ) ) 

*BETA1+ALPHA1~BETA1 ) 

IF ( T . GE . TR2) PHIDT2=-RATE/ ( ALPHA 1 -BETA 1 ) *( -EXP 
(-ALPHA#(T-TR2) )*ALPHA1+EXP<-ALPBET*(T-TR2) ) 

♦BETA1+ALPHA1-BETA1 ) 

IF(T.GE.TFl) PHIDT1  =  PHIDT1-RATE/ ( ALPHA 1 -  BETA 1 ) * 

( -EXP ( -ALPHA* ( T-TF 1 ) ) *ALPHA1 +EXP ( -ALPBET* ( T-TF1 ) ) 
*BETA1+ALPHA1-BETA1 ) 

IF ( T . GE . TF2)PHIDT2=PHIDT2+RATE/ ( ALPHA 1 -BETA 1 )* 

< -EXP ( -ALPHA* ( T-TF2 )  >*AL.PHA1+EXP(  -ALPBET* (T-TF2) ) 

#BETA1+AL  PHA1-BETA1 ) 

PH IDT =PHIDT1+PHIDT2 

IF ( T . GE . TR1 ) PH 1 1 =RATE/ ( ALPHA 1 -BETA 1 ) * ( EXP  < -ALPHA* 

( T-TR1 ) ) *ALPHA1*ALPHA 1 -EXP ( -ALPBET* ( T-TR 1 ) ) * 

BETA.1*BETA1  +  ( T-TR1  )*ALPHA1-  (  T-TR1 )  *BETA1-ALPHA1 
*ALPHA1+BETA1*BETA1 ) 

I F ( T . GE . TR2 ) PHI 2=-RATE/ ( ALPHA 1 -BETA1 ) * ( EXP ( -ALPHA* 

( T-TR2 ) ) #ALPHA1*ALPHA1-EXP ( -ALPBET* ( T-TR2) ) * 

BETAl*BETAl  +  ( T-TR2)*ALPHAl-<  T-TR2)*BETA1-ALPHA1 
*ALPHA1+BETA1*BETA1 ) 

IF ( T . GE . TF1 )PHI 1=PHI l-RATE/<  ALPHA 1 -BET A 1 >*<  EXP 


w  ro  h  w  ro 


( -ALPHA* ( T-TF1 ) ) *ALF'HA1*ALF'HA1-EXP ( -ALPBET* <  T- 
TF1 ) )#BETA1#BETA1+(T-TF1 >*ALF'HA1 -(T-TF1 )#BETA1 
-ALPHA!  *ALF'HAH  BETA!  #BETA1 ) 

IF  (T.GE.TF2)  PHI  2=PHI2+RATE/<  ALF'HAl-BETAl )*<EXP 

<  -ALPHA*  <  T-TF2 )  >  *ALPHA1  *ALF'HA1  -EXP  (  -ALPBET*  ( T- 
TF2 ) ) #BETA1*BETA1 F ( T-TF2 ) *ALPHA1- ( T-TF2 ) *BETA1 
-ALPHA  1  *ALPHAHBET  A1  *BET  A 1 ) 

PHI=PHI1+PHI2 

GENERATE  TURN  INPUTS 

TI.JRND=32 . 2*TAN  ( PHI )  /TAS 
TURN=TURN+TURND#DT2 
IF  ( TRATE  .  L.T .  0  ♦  )  GO  TO  75 

IF  <  T .GE.TR3) TURND1  =  TRATE/ <  ALPHA 1 -BETA 1 ) *( -EXP 
( -ALPHA* ( T-TR3 ) ) *ALF'HA1+EXP < -ALPBET* ( T -TR3 ) ) * 
BETAl+ALF'HAl-BETAl) 

IF ( T ♦ GE . TR4 ) TURND2= -TRATE/ ( ALPHA1 -BET A1 >  * ( -EXP 
( -ALPHA* ( T-TR4 ) ) *ALPHA 1 +EXP < -ALPBET*  <  T -TR4 ) ) * 
BETA1 + ALPHA! -BETA 1 ) 

IF  <  T  » GE . TF3) TURND1 =TURND1 -TRATE/ ( ALPHA1 -BETA 1 ) * 

( -EXP  ( -ALPHA*  <  T-TF3 ) )  *ALPHA1  +EXF*  ( -ALPBET*  <  T-TF3 )  ) 
*BETA1+ALPHA1 -BETA1 ) 

IF  ( T  .  GE  .  TF4 )  TURND2=TURND2+TRATE/  ( ALF'HAl-BETAl ) * 

( -EXP  < -ALPHA* ( T-TF4 ) ) *ALPHA1 +EXP  < -ALPBET* ( T-TF4 ) ) 
*BETA1+ALPHA1~BETA1 ) 

TURND»TURND1+TURND2 

IF  < T  .  GF. . TR3 )  TURN1=TRATE/ ( ALF'HAl-BETAl )  * ( EXP 
( -ALPHA*  ( T-TR3 )  )  *  ALPHA  1#ALF’HA1 -EXP  (  -ALPBET* 

<  T-TR3) ) #BETA1*BETA1  +  <  T-TR3 >*ALPHA1- 

( T-TR3 ) *BETA1-ALPHA1*ALPHA1 +BETA1*BETA1 ) 

IF  (  T  .  GE .  TR4 > TURN2=-TRATE/ ( ALF'HAl-BETAl  > *  ( EXP 
( -ALPHA*  (  T-TR4  >  )*ALPHA1*ALF'HA1  -EXP  (  -ALPBET* 

( T-TR4 )  )  *BETAl*BETAl  +  <  T-TR4 )  *ALF’HA1- 
(T-TR4)*BETA1 -ALPHA  1*ALF'HA1+BETA1*BETA1> 

IF  (T.GE.TF3)TURNl  =  TURNl-TRATE/(  ALF'HAl-BETAl) 

* ( EXP ( -ALPHA* (T-TF3 ) ) *ALPHA 1 *ALPHA1 -EXP 
< -ALPBET#  <  T-TF3 ) ) *BETA1#BETA1+  <  T-TF3) * 

ALF'HAl  -  (  T-TF3 )  #BETA1  -ALPHA  1  #ALPHA1FBETA1*BETA1 ) 

IF  (T.GE.TF4)TURN2=TURN2+TRATE/(  ALF'HAl-BETAl) 

* ( EXP ( -ALPHA* (T-TF4)  ) *ALPHA1  *ALF'HA  1  -EXP 
< -ALPBET# (T-TF4) ) *BETA1*BETA1+ ( T-TF4 ) * 

Al.PHAl-<  T-TF4 )  *BE  TA1 -ALPHA  1*ALF’HA1  +  BETA1*BETA1 ) 
TURN=TURN1+TURN2 


TARGET  MANUEUER 


IF ( HORTG . EG ♦ 0 . ) GO  TO  80 
TGEES=0 « 

IF ( T ♦ GE . HORT 1 ) TGEES^HORTG#  < 1 . -EXP( -DELTA# IT-H0RT1 ) ) ) 
IF  <  T . GE . H0RT2 ) TGEE5=H0RTG*EXP  < -DELTA* ( T-H0RT2 > ) 
TANG=ATAN2 ( XTCON » YTCON ) 

XTC0N=XTC0N-32 . 2#TGEES*C0S  <  TANG ) *DT2*DT2 


YTCON-YTCON+32 . 2*TGEES*SIN ( TANG ) *DT2*DT2 
C 

C  UPDATE  GEOMETRY 

C 

80  XFDIST=:XFDIST+C0S<TURN>*TAS*DT2 

YFDIST  =  YFDIST+SIN  <  TURN ) #T AS#DT2 
XTDIST=XTDIST+XTCON 
YTDIST=YTDIST+YTCON 

ANGL.E=ATAN  (  ( YTDIST-YFDIST )  /  <  XTDIST-XFDIST )  ) 

RANGE=SGRT  ( <  XTDIST-XFDIST )  **2 .  +  <  YTDIST-YFDIST)  #*2 .  > 

RDOT  = <  RANGE-RANGER ) /DT2 
RAN6EP=RANGE 
C 

100  RETURN 

END 
C 
C 

SUBROUTINE  RK4 < M ,N , TT , I I ) 

C 

C******  FOURTH  ORDER  RUNGE-KUTTA  ROUTINE  *##*### 

C 

COMMON/STEP/T , DT , DT2 

C0MM0N/RADAR/Y<2,5) ,Y1<2,5) ,Y2<2,5) , XI (2,2) ,0UT(2,2), 

1  A,B,C,D,E,GA,KA,K,C0EF1,C0EF2,C0EF3,C0EF4,C0EF5,C0EF6, 

2  XX(3) ,YY(3) ,MODE, IRADAR 

COMMON/AIR/RATE, PHI, PHIDT,PPHI, PITCH, PITCHD,PPITCH,TRATE, 

1  TURN , TURND , PTURN , TR 1 , TR2 ,  TR3 , TR4  ,  TF 1 , TF2 , TF3 , TF4 , ANGLE , 

2  ASPECT , RANGE , RDOT , RANGER , AOA , T AS , TAST  ,  ALPHA , BETA , ALPHA 1 

3  ALPBET , BETA1 , XFDIST , YFDIST , XTDIST  , YTDIST ,  XTCON, YTCON  , 

4  H0RT1 ,H0RT2,H0RTG, TGEES, DELTA, IAIR, TANG 
C0MM0N/RK4SAV/SAV  <  2 , 5 ) 

REAL  K , KA 
C 

GO  TO  (200,210,220,230) ,M 

200  DO  201  I =1 , N 
Y2(II,I)=Y<II,I) 

C0=DT2*Y1(II,I) 

Y ( I I , I ) =Y2 ( I I , I ) +C0 

201  SAU ( I I , I ) =CO+CO 
TT=TT+DT2 
RETURN 

210  DO  211  1=1, N 
C0=DT2*Y1(II,I) 

Y ( I I , I )=Y2< I I , I )+C0 

211  SAV< 1 1 , 1 ) =SAV< 1 1 , 1 ) +4  »0#C0 
RETURN 

220  DO  221  1  =  1, N 
C0=DT*Y1 ( 1 1 , 1 ) 

Y ( I I , I ) =Y2 ( I I , I ) +C0 

221  SAV< II , I )=SAV< II , I )+CO+CQ 
TT  =  TT  FDT2 

RETURN 

230  DO  231  1*1,  N 

231  Y(II,I)=Y2(II,I)+(SAV<II,I)+DT*Yl(II,I))/6.0 
RETURN 

END 


)|(  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  i^<  ^  ^  ^  j|^  ^  ^  ^  j|^  ^  ^  ^  \y  ^  y^p  t|^  ^  .^»  ^  ^p  ^  ^  ^  j^p  j^p  ^  ^  ^p  j|^  ^p  ^|p  ^  >^p  ^p  j^p  ^  ^ 

*******  TEST  DEFINITION  FILE  FOR  RADAR. FTN  PROGRAMS  ******* 

BEAM  ANGLE  0  DEG,  LAG 
0  {MODE  ( 0 -ACM , 1-LRI > 

0  JNDATA-INPUT  DATA  (O-INTERNAL,  1-EXTERNAL) 

.02  , DT-TIME  STEP 

1  J NTSOUT-NO .  TIME  STEPS/OUTPUT 

o.  , 'tine-initial  time 

15. so  jtfnl-final  time 

1.  ; RATE-ROLL  RATE 

5..  10.  JALPHA, BETA-EXPONENTIAL  CONSTANTS  FOR  ROLL  AND  TURN 

3 . .  4 .  JTR1,TF1 -FORWARD  START, STOP  TIMES 

5..  6.  JTR2,TF2-REVERSE  START, STOP  TIMES 

.785398163  JANGLE-INITIAL  AZIMUTH  ANGLE  TO  TARGET 

3.5  J FILTER  CONSTANT 

.785398163  J ASPECT  ANGLE  OR  RDOT-RANGE  RATE 

40000.  J  RANGE  TO  TARGET 

800..  800.  J TRUE  AIR  SPEED-FIGHTER , TARGET 

-1.  J TURN  RATE  (NEGATIVE  FOR  AUTO  RATE) 

0.,0.  t TR3 , TF3-RIGHT  TURN  START, STOP  TIMES  (MANUAL) 

0.,0.  JTR4,TF4-LEFT  TURN  START, STOP  TIMES  (MANUAL) 

0. , .0001 ,0. , 18.85  JSNl ,SN2,SN3,FR 

0  $  IF  1, FORMATTED  OUTPUT , OTHERWISE  UNFORMATTED 

1 .  , 9  ,  ;H0RT1,H0RT2--H0RIZ0NTAL  turn  start, stop  times 

3..  5.  , *HORTG(+AWAY, -TOWARDS) , DELTA-TARGET  GEES,  EXPONENTIAL 


W  W  V  w  ^  ^  ■  1 1  '  '  i »  ^  ^  'L>  <  L>  ib  »  L»  >b  ^  i  b  ^  \  b/  ^  ^  ^  rl<  ^  ^  ^  I L*  W  •  L>  >  b  ^  ^  -b  ^  w  ib  ^  ^  w  w  w  ^  ^  ^  ■!>  ^  <  I  >  .  |.  .  b  ^L*  \b 

'p  *  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ip  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  /*  ^  ^  ^  ^  ^  ^  ^ 

*******  TEST  DEFINITION  F  ILE  FOR  RADAR  * FTN  PROGRAMS  ******* 

TAIL  CHASE  TRAJ 

0  {MODE  (O-ACM,  1-L.RI) 

0  {NDATA- INPUT  DATA  (O-INTERNAL,  1-EXTERNAL) 

.02  {DT-TIME  STEP 

1  { NTS01JT-N0 .  TIME  STEPS/OUTPUT 

0.  {T INI. -INITIAL  TIME 

15.30  { TFNL-FINAL.  TIME 

1.  {RATE-ROLL  RATE 

5..  10.  {ALPHA, BETA-EXPONENTIAL.  CONSTANTS  FOR  ROLL  AND  TURN 

3..  6.  { TR1 , TF1-F0RWARD  START, STOP  TIMES 

16..  16.  { TR2 , TF2 -REVERSE  START, STOP  TIMES 

-.087266  {ANGLE-INITIAL  AZIMUTH  ANGLE  TO  TARGET 

3.5  {FILTER  CONSTANT 

3.228859  {ASPECT  ANGLE  OR  ROOT -RANGE  RATE 

10000.  {RANGE  TO  TARGET 

300..  800.  {TRUE  AIR  SPEED-FIGHTER , TARGET 

-1.  {TURN  RATE  <  NEGATIVE  FOR  AUTO  RATE) 

0.,0.  }TR3,TF3-RIGHT  TURN  START, STOP  TIMES  (MANUAL) 

0.,0.  {TR4,TF4-LEFT  TURN  START, STOP  TIMES  (MANUAL) 

0. , .0001,0. , 18.85  {SN1 ,SN2,SN3,FR 

1  {IF  1, FORMATTED  OUTPUT, OTHERWISE  UNFORMATTED 

1 . . 16.  {H0RT1 ,HORT2-HORIZONTAL  TURN  START, STOP  TIMES 

3..  5.  { HGRTG(+AWAY, -TOWARDS >, DELTA-TARGET  GEES,  EXPONENT  I  Al. 

•-- EOR— 
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Appendix  C 

SOFE  User-Written  Programs 


♦DECK  FGGEN 

SUBROUTINE  FQGEN  (IRUN,  T,  NF  ,  NS,  NXTJ,  XF ,  XS,  XTRAJ,  NZF , 
+  F  ,  OF ) 

+++  =FGGEN=,  A  USER-WRITTEN  SUBROUTINE  FOR  SOFE, 

+++  CALLED  ON  DEMAND  OF  THE  INTEGRATOR,  SUPPLIES  THE  NONZERO 
+++  ELEMENTS  OF  THE  MATRICES  F  AND  GF  FOR  PROPAGATION  OF  THE 
+  +  +  COVARIANCE  MATRIX  PF ,  THERE  ARE  NOT  ANY  TIME  VARYING  VALUES 
+++  OF  MATRIX  F  BECAUSE  OF  LINEAR  DYNAMICS  —  SO  RETURN. 

COMMON/QF/QFIN  <  3) 

DIMENSION  XF(NF),  XS(NS),  XTRAJ(NXTJ),  F ( NZF ) ,  GF(NZQ) 

RETURN 

♦  FGGEN  ENTRY  POINT  * 

ENTRY  FGGENO  (IRUN,  T,  NF,  NS,  NXTJ,  XF,  XS,  XTRAJ,  NZF,  NZG 
+  F ,  GF ) 


F  <  1 )  =  1 . 

F(2)=l . 

F(3)=-7. 

F  (  4  ) '- 1  . 

F  ( 3 )  =  1 . 

F ( 6 ) ~-7 , 

F  (  7 )  =  1  . 

F(8)  =  l  . 

F(9)=-7. 

GF ( 1 ) =QFIN ( 1 ) 

QF(2)-GFIN(2) 

GF (3)=QFIN<3) 

RETURN 
END 
DECK  HRZ 

SUBROUTINE  HRZ  (IRUN,  T,  NF ,  NS,  NXTJ,  XF,  XS,  XTRAJ,  NTR ,  PF 
+  IMEAS,  M,  H,  RF,  ZRES) 

+++  =HRZ=,  A  USER-WRITTEN  SUBROUTINE  FOR  SOFE. 

+++  CALLED  M  TIMES  AT  DTMEAS  INTERVALS.  SUPPLIES  SENSITIVITY 
+++  VECTOR  H,  NOISE  VARIANCE  RF,  AND  RESIDUAL  ZRES. 

+++  MAKE  RF  <  0  TO  SUPPRESS  A  MEASUREMENT. 

COMMON/RFCOM/RFIN( 6) 

COMMON/HHCOM/HH(  24 ) 

DIMENSION  XF(NF),  XS(NS),  XTRAJ(NXTJ),  PF ( NTR) ,  H(NF),STD(6) 

FIRST,  EVALUATE  H  FOR  ALL  MEASUREMENTS 
IF  (IMEAS  .EG,  1)  THEN 


SUMSQ  = 
SUMSQ12 
SUMSQ32 
SUMSQ2 
F'SUM 
PSUM 12 
PSUM2 
PSUM32 
AUDI 
ADD2 
ADD3 
XF(4)*XF 
HH(  1 ) 

HH  ( 2 ) 

HH  ( 3 ) 

HH  (  4 ) 

HH  <  5 ) 

HH  <  6 ) 

HH  ( 7  ) 

HH  ( 8 ) 

HH  ( 9  ) 
HH<10) 
HH  <11) 
HH< 12) 
HH( 13) 
HH< 14) 
HH< 15) 

HH (16) 
HH ( 17 ) 

HH( 18) 
HH( 19) 


HH  <  20 ) 
HH (21 ) 

HH ( 22 ) 
HH(23) 


HH ( 24 ) 
END  IF 


XF<1>#*2  +  XF  <  4 ) **2  +  XF<7)**2 
=  SORT (SUMSQ) 

=  (SUMSQ) **1.5 
=  ( SUMSQ )**2 
=  XF ( 1 ) **2  +  XF(4)**2 
=  SORT  ( F’SUM) 

=  PSUM**2 
=  PSUM**1.5 

=  ( XF ( 1 ) * ( XF ( 2 ) -XTRAJ ( 16 ) ) ) +XF ( 4 ) *XF ( 5 )  +  XF(7)*XF<8) 

=  XF ( 4 ) **2  -  XF ( 1 ) **2 

=  XF ( 8 ) *PSUM  -  XF(7)*(XF(1)*(XF ( 2) -XTRAJ ( 16) )  + 

(5)  ) 

=  XF ( 1 ) /SUMSQ12 
=  XF ( 4 ) /SUMSQ1 2 
=  XF (7)/SUMSQ12 

=  ( (XF(2)~XTRAJ(  16)  )*SUMSQ  --  XF ( 1 ) *ADD1 ) /SUMSQ32 
=  HH  ( 1 ) 

=  ( XF ( 5 ) *SUMSQ  -  XF(4)*ADD1)/SUMSQ32 
=  HH ( 2 ) 

=  ( XF ( 8 ) *SUMSQ  -  XF(7)*ADD1 )/SUMSQ32 
=  HH(3) 

=  -XF ( 4 ) /PSUM 
=•  XF  ( 1 )  /F'SUM 

=  (XF(1)*XF(7))/(SUMSQ*PSUM12) 

=  ( XF  ( 4  )  *XF  ( 7 )  )  /  ( SUMSQ*F‘SUM12 ) 

■  -PSUM12/SUMSQ 

*  ( XF ( 5 ) *ADD2  +  2,*XF(1)*(XF(2)-XTRAJ(16) >*XF(4)  ) 
/PSUM2 
=  HH<10) 

=  ( (XF(2) -XTRAJ(16) )*ADD2  -  2 . *XF ( 1 ) *XF ( 4 ) *XF ( 5 ) ) 
/PSUM2 
-  HH  (11) 

(  <  SUMSQ*PSUM*  <  2 .  *XF  ( 1 )  *XF  ( 8 )  - 
( XF < 2 ) -XTRAJ ( 16))*XF<7))>  -  ADD3* 

XF ( 1 ) * < 2 . #F'SUM  +  SUMSQ)  )/(SUMSQ2*PSUM32) 

=  HH(  12 ) 

=  -(  (SUMSQ*F'SUM*(2.*XF(4>*XF(8)  -  XF ( 5 ) #XF ( 7 ) ) )  -  ADD3* 
XF ( 4 ) *  <  2 . *PSUM  +  SUMSQ) )/(SUMSQ2*PSUM32) 

=  HH( 13) 

=  <  < SUMSQ* PSUM* (XF(1)*(XF<2) -XTRAJ ( 16) )  + 

XF  <  4 ) *XF ( 5 )  )  +  ADD3* 

2 .  *XF  ( 7 )  *F'SUM ) )  /  <  SUMSQ2*PS(JM32 ) 

»  -PSUM/  ( SUMSQ*F'SUM12 ) 


NOW  GET  RF 

RF  -  RFIN(IMEAS) 


NOW  OBTAIN  MEASUREMENT  RESIDUALS,  SEQUENTUALLY , 
DO  5  1=1, NF 
H< I ) =0 ♦ 

5  CONTINUE 


( ZRES  = 


RANGE 
IF  (IMEAS 
ZRES  * 
H  <  1 )  = 
H  <  4 )  = 
H  ( 7 )  = 
END  IF 


. EQ .  1)  THEN 
XTRAJ(l)  -  SUMSG12 
HH  ( 1 ) 

HH(2) 

HH(3) 


GAUSS  <  0  * ,STD(1>  > 


RANGE  RATE 


IF  (IMEAS  .EG.  2)  THEN 

ZRES  =  XTRAJ<2>  -  ADD1/SUMSG12 
H ( 1 >  =  HH( 4 ) 

H ( 2 )  -  HH ( 5 ) 

H ( 4 )  =  HH  <  6 ) 

H ( 5 )  =  HH ( 7 ) 

H ( 7 )  =  HH  <  8 ) 

H ( 8 )  =  HH ( 9 ) 

END  IF 


GAUSS(0, fSTD(2> ) 


AZIMUTH  ANGLE 
IF  (IMEAS  .EG. 
ZRES  =  XTRA 
H ( 1 )  =  HH( 1 
H  (  4  )  -•  HH  ( 1 
ENDIF 


.EG.  3)  THEN 

XTRA J ( 3 )  -ATAN(XF(4)/XF(1) ) 
HH  (10) 

HH  (11) 


GAUSS ( 0 . , STD ( 3 )  ) 


ELEVATION  ANGLE 
IF  (IMEAS  .EG.  4) 
ZRES  =  XTRAJ ( 4 


H(  1) 
H  ( 4 ) 
H  ( 7 ) 
ENDIF 


.EG.  4)  THEN 

XTRAJ ( 4 )  +  ATAN(XF  (7)/F'SUM12> 
HH( 12) 

HH  ( 13 ) 

HH( 14) 


GAUSS ( 0 . r STD ( 4  )  ) 


AZIMUTH  RATE 
IF  (IMEAS  .EG.  5)  THEN 

ZRES  =  -(XF(1)*XF(5>  -  ( XF  ( 2 ) -XTRAJ ( 16 ) ) *XF ( 4  )  ) /F'SUM 
+XTRAJ(5)  +  GAUSS (0»  *STEK  5 )  ) 

H ( 1 )  =  HH( 15) 

H( 2)  =  HH( 16) 

H ( 4 )  =  HH ( 17 ) 

H( 5)  =  HH( 18) 

ENDIF 


ELEVATION  RATE 
IF( IMEAS  .EG,  6 ) 


THEN 


ZRES  = 


H  ( 1  > 
H  ( 2) 
H  ( 4 ) 
H  ( 5 ) 


(XF(8)*PSUM  - 
XF ( 4)tXF ( 5) ) / 
(SUMSG*PSUM12) 
HH ( 1 9 ) 

HH ( 20 ) 

HH( 21 ) 


XF(7)*(XF( 1 >*(XF(2)-XTRAJ( 16) ) ) 


GAUSS ( 0 ♦ » STD ( 6  > )  +  XTRAJ(6) 


H  <  8 )  =  HH ( 24 ) 

ENDIF 

C 

RETURN 

C 

C  *  HRZ  ENTRY  POINT  * 

C 

ENTRY  HRZO  (IRUN,  T,  NF ,  NS,  NXTJ,  XF,  XS,  XTRAJ,  NTR , 

+  IMEAS,  M,  H,  RF,  ZRES) 

STD ( 1 )  =  SORT  ( RFIN< 1 ) ) 

STD ( 2 )  =  SORT ( RFIN  <  2 ) ) 

STD ( 3 )  =  SORT ( RFIN ( 3 ) ) 

STD ( 4 )  =  SORT ( RFIN <  4 ) ) 

STD (5)  =  SORT ( RFIN  <  5 ) > 

STD ( 6 )  -  SORT <RFIN< A)  ) 

RETURN 

END 

♦DECK  TRAJO 

SUBROUTINE  TRAJO  <IRUN,  T,  NF,  NS,  NXTJ,  XF,  XS,  XTRAJ) 
C 
C 

C  +++  =TRAJO= ,  A  USER-WRITTEN  SUBROUTINE  FOR  SDFE , 

C  +++  CALLED  FROM  USER  ROUTINES  FOR  INTERNAL  TRAJECTORY 
C  +++  CONSTRUCTION.  MANDATORY  FOR  READING  TAPE  HEADER 
C  +++  AT  TO  IF  TRAJECTORY  IS  STORED  EXTERNALLY  ON  TAPE. 

C 

DIMENSION  XF < NF ) ,  XS<NS>,  XTRAJ(NXTJ) 

CHARACTER  TITLE*40,MODE*20,NAME*30 

C 

C  TRAJO  READS  AND  ECHOS  TITLE, NAME,  AND  MODE 

READ(3)TITLE 
READ<3)NAME 
READ  <  3 ) MODE 
IF  ( IRUN  , NE  «  DRETURN 
C  PRINT  TITLE,  NAME,  AND  MODE 

WRITE(6,1000)TITLE 
WRITE (A, 1000) NAME 
WRITE (6, 1000) MODE 
RETURN 

1000  FORMAT ( A40 ) 

END 

♦DECK  SNOYS 

SUBROUTINE  SNOYS  (IRUN,  T,  NF,  NS,  NXTJ,  XF ,  XS,  XTRAJ) 
C 
C 

C  +++  --SNOYS* ,  A  USER-WRITTEN  SUBROUTINE  FOR  SOFE . 

C  +++  CALLED  AT  DTNOYS  INTERVALS.  PERTURBS  TRUTH  STATES  WITH 
C  +++  RANDOM  NOISE  SAMPLES  TO  SIMULATE  ACCUMULATED  EFFECT  OF 
C  +++  PROCESS  DRIVING  NOISE  OVER  THE  INTERVAL  DTNOYS, 

C  +++  FOR  THIS  FILTER,  THE  ABOVE  DOES  NOT  PERTAIN.  RATHER, 


nnnonn  non 


C  +  H-  THE  TRUTH  STATES  ARE  READ  INTO  XS  AT  DTNOYS  INTERVALS. 

C 

DIMENSION  XF(NF),  XS(NS),  XTRAJ(NXTJ) 

FIRST  OBTAIN  TRUE  STATES  XS  FROM  XTRAJ 

XS(1 >=XTRAJ<7) 

XS  <  2 ) =XTRA J  <  8 ) 

XS(3)=XTRAJ(9) 

XS  <  4 ) = XTRAJ ( 10 ) 

XS(5)=XTRAJ( 11 ) 

XS ( A ) =XTRAJ ( 12 ) 

XS(7 )=XTRAJ( 13) 

XS<8)=XTRAJ( 14) 

XS(9)=XTRAJ( 15) 

RETURN 

*  SNOYS  ENTRY  POINT  * 

ENTRY  SNOYSO  (IRUN,  T,  NF ,  NS,  NXTJ,  XF,  XS,  XTRAJ) 

C 

RETURN 

END 

*DECK  USR.TN 

SUBROUTINE  USRIN 
C 
C 

C  +  +  +  ==USRIN= ,  A  USER-WRITTEN  SUBROUTINE  FOR  SOFE . 

C  +++  CALLED  ONCE  AT  THE  BEGINNING  OF  THE  PROBLEM,  USEFUL 
C  +++  FOR  READING  IN  AND  ECHOING  PARAMETERS  THAT  DEFINE 
C  +++  USER'S  PROBLEM.  TRACKER  PROBLEM  WITH  LINEAR  DYNAMICS  AND 
C  +++  NONLINEAR  MEASUREMENTS. 

C 

COMMON/GF/GFIN(3) 

COMMON/RFCOM/RFIN<  6 ) 

COMMON/ROTCOM/ROTATE (9,9) 

COMMON/NCOM/NF , NS , M , NZF , NZG , NXTJ , NTR , NALL 

NAMELIST/IN/QFIN,RFIN 

READ (5, IN) 

WRITE<6, IN) 

C  ZEROIZE  ROTATE  MATRIX  (USED  IN  SUBROUTINE  S0FM0D1) 

DO  50  1=1, NF 
DO  40  J=1 ,NF 
RQTATE( I , J)=0. 

40  CONTINUE 
50  CONTINUE 
RETURN 
END 

♦DECK  XFDOT 

SUBROUTINE  XFDOT  (IRON,  T,  NF,  NS,  NXTJ,  XF,  XS,  XTRAJ,  NTR,  PF 


XDOT ) 


C  +  +  +  -XFDOT  =  ,  A  USER-WRITTEN  SUBROUTINE  FOR  SOFE . 

C  +++  CALLED  ON  DEMAND  OF  THE  INTEGRATOR.  SUPPLIES  THE 
C  +++  DERIVATIVE  OF  THE  ESTIMATED  FILTER  STATE  VECTOR  ( XFDOT =F ( XF , T  )  )  . 

C 

CQMMON/OLD/TURNOLD ,  PHI OLD 

DIMENSION  XF(NF),  XS(NS),  XTRAJ(NXTJ),  PF < NTR ) ,  XDOT < NF ) 

C 

XDOT < 1 )  =  XF ( 2 )  -  XTRAJ< 16) 

XDOT ( 2 )  =  XF( 3) 

XDOT (3)  =  -7.*XF<3) 

XD0T(4)  =  XF ( 5 )  -  XTRAJ( 17 ) 

XD0T(5)  =  XF  ( 6) 

XDOT  <  6 )  =  -7.*XF(6) 

XDOT ( 7 )  =  XF ( 8)  -  XTRAJ< 18) 

XDOT(8)  =  XF  ( 9 ) 

XDOT ( 9 )  =  -7.*XF(9) 

RETURN 

C 

C  *  XFDOT  ENTRY  POINT  * 

C 

ENTRY  XFDOTO  (IRUN,  T,  NF,  NS,  NXTJ,  XF,  XS,  XTRAJ,  NTR,  PF , 

+  XDOT ) 

C 

C  SAVE  INITIAL  VALUES  OF  TURNOLD  AND  PHIOLD  FOR  SUBROUTINE  SOFMOD1 . 

TURNOLD  =  XTRAJ (19) 

PHIOLD  =  XTRAJ(20) 

C  GET  INITIAL  ESTIMATES  FROM  EXTERNAL  TRAJECTORY 

XF( 1 >=XTRAJ<7) 

XF ( 2) =XTRAJ  <  8 ) 

XF (3)=XTRAJ(9) 

XF ( 4 ) =XTRA J ( 10 ) 

XF  (5)=XTRAJ( 11 ) 

XF ( 6 ) =XTRAJ ( 12 ) 

XF(7)=XTRAJ(13) 

XF ( 8 ) =XTRAJ ( 14 ) 

XF ( 9) =XTRAJ( 15) 

RETURN 

END 

♦DECK  XSDOT 

SUBROUTINE  XSDOT  (IRUN,  T,  NF,  NS,  NXTJ,  XF,  XS,  XTRAJ,  XDOT) 


-~XSDQT=,  A  USER-WRITTEN  SUBROUTINE  FOR  SOFE, 

CALLED  ON  DEMAND  OF  THE  INTEGRATOR.  SUPPLIES  THE  HOMOGENEOUS 
PART  OF  THE  DERIVATIVE  OF  THE  TRUTH  STATE  VECTOR.  SINCE  THE 
TRUTH  STATE  XS  IS  OBTAINED  FROM  THE  EXTERNAL  TRAJECTORY, 

JUST  DEFAULT. 
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DIMENSION  XF(NF),  XS(NS),  XTRAJ(NXTJ),  XDOT(NS) 

XDOT(l)  =  0, 

XD0T<2)  =  0. 

XD0T(3)  =  0, 

XEiOT  <  4 )  =  0. 

XD0T(5)  =  0. 

XDOT  <6)  =  0* 

XDOT <  7 )  =  0. 

XD0T(8)  =  0. 

XDOT (9)  =  0. 

RETURN 

*  XSDOT  ENTRY  POINT  # 

ENTRY  XSDOTO  (IRUN,  T,  NF,  NS,  NXTJ,  XF,  XS,  XTRAJ,  XDOT) 


XS(1)  = 
XS(2)  = 
XS(3)  = 
XS  (4)  = 
XS<  5  )  = 
XS  <  6 )  = 
XS<  7  )  = 
XS  ( 8 )  = 
XS<9 )  = 
RETURN 
END 


XTRAJI7) 
XTRAJ(8) 
XTRAJ<  9 ) 
XTRAJ(IO) 
XTRAJ(ll) 
XTRAJ< 12) 
XTRAJ< 13) 
XTRAJ< 14) 
XTRAJ< 15 ) 


' 1 


SOFE  Job  Control 


APQl  20  ,  P3 . 

USER , T850420 , ROSSULD . 
CHARGE,#. 

SETTL , 1000 . 

GET ,USERBNA. 

GET , SOFEB . 

GET ,  TAF'E5~S0FEDAT . 

GET , TAPE3=0UTBEAM . 

GET ,MTHLIB7 . 

LIBRARY , MTHLIB7 , 

ATTACH , TAPE4=KFTEHP/M=W 
LOAD ( USERBNA , SOFEB ) . 
EXECUTE ( , *PL=1 00000 ) . 


SOFEPL  Job  Control 


APQPLOT  tP3. 

USER f  T850420  ,  ROSSULB ► 

CHARGE , * . 

SETTL  f 750  » 

RETURN  f META  ♦ 

GET , LG0=S0FEPLB/UN=V780355 . 
ATTACH , D ISSPL A/UN= APPL I B . 
ATTACH , TAPE4=KFTEMP . 

GET  t  TAPE5=SF'LI«ATB. 

ATTACH , META=META1/M=W . 

LDSET  »LIB=EHSSPLA» PRESET =0 . 
LGO<  ,*PL  =  1 00000)  . 


C-9 


APPENDIX  D 
SOFE  Modification 

As  explained  in  Section  4.3.3,  the  state  estimate  and 
filter  covariance  must  be  rotated  into  the  update  frame  at 
time  ti+  from  the  propagation  frame  at  time  t^~  to  compensate 

for  any  changes  in  fighter  heading,  pitch,  or  roll  during  the 

interval  from  t.  ,+  to  t.~.  The  rotations  are  accomplished 
i-l  i  r 

according  to  the  following  relations: 


E<ci>new  ’  ^(ti  > 
£(ti'»new  ' 


(D-l) 
(D-2 ) 


where  C  is  a  direction  cosine  matrix.  Using  Equation  (2-4), 


C  is  formed  as 


[C]  =  [A^][A0][Affl] 


For  the  truth  model  used,  A0  equals  zero  so  the  above 
relation  reduces  to 


[C]  =  [A«J][AW] 


where 


( D-3 ) 


(D-4) 


c  ( AW )  s  ( AID)  0 

[  AID  ]  =  -s(ADI)  c  ( AID )  0 


8 


Sf 


£ 


H 


c  =  cosine,  and 


s  =  sine 


For  a  three-by-three  matrix 


c  ( AflJ ) 


s(AlD) 


[C]  =  -c(AdS)sUD)  c  ( A^  )  c  ( Al )  s(A«5) 

s  ( A$ )  s  ( AID )  -s  ( A^)C(AflJ)  c(A«S) 


(D-5 ) 


Similarly,  expanding  to  a  nine-by-nine  matrix  results  in 


Cll 

0 

0 

C12 

0 

0 

0 

0 

0 

0 

Cll 

0 

0 

C12 

0 

0 

0 

0 

0 

0 

Cll 

0 

0 

C12 

0 

0 

0 

C21 

0 

0 

C22 

0 

0 

C23 

0 

0 

0 

C21 

0 

0 

C22 

0 

0 

C23 

0 

0 

0 

C21 

0 

0 

C22 

0 

0 

C23 

C31 

0 

0 

C32 

0 

0 

C33 

0 

0 

0 

C31 

0 

0 

C32 

0 

0 

C33 

0 

0 

0 

C31 

0 

0 

C32 

0 

0 

C33 

(D-6) 


where 


=  c  ( AID ) 

=  s(Afll) 

=  -c(A^)s(®) 


•.ii® 


msmmFm 


C22  =  c(A(Zi)c(Af) 

C23  =  s(A«J) 

/•v./ 

%  ^ 

C31  =  s(A<J)s(Affl) 

C32  =  -s(A«S)c(AIB) 

C33  =  c(A(£) 

Finally,  page  D-4  illustrates  where  the  rotation  routine  is 
inserted  in  SOFE  and  pages  D-5  through  D-8  illustrate  how 
Equation  (D-6)  is  incorporated  into  the  source  code. 


SOFE  Modification  Call 


» 


END 

♦DECK  MCRUN 

SUBROUTINE  MCRUN  (T) 

C 

C  +++  =MCRUN=  IS  THE  CONTROLLING  ROUTINE  FOR  A  MONTE  CARLO  RUN.  =MCRUN= 
C  +++  IS  EITHER  INTEGRATING  OR  UPDATING.  WHEN  INTEGRATING,  =MCRUN= 

C  +++  RELIES  ON  SUBROUTINE  =ADVANS=  TO  SCHEDULE  AND  ACCOMPLISH  ALL  (SIX 
C  +++  SOFE  EVENTS  EXCEPT  MEASUREMENT  UPDATE.  AT  AN  UPDATE  TIME,  T-, 

C  +++  ~ADVANS=  RETURNS  TO  =MCRUN=  WHERE  THE  VARIOUS  UPDATE  SUBROUTINES 
C  +++  ARE  CALLED  IN  SEQUENCE  TO  REACH  TIME  T+C.  WHEN  T  REACHES  TF,  THE 
C  +»+  FINAL  TIME  OF  A  RUN,  ~MCRUN=  RETURNS  CONTROL  TO  =SOFE=. 

C 

COMMON  A  ( 1 ) 

COMMON  /  ICOM  /  ICONT,  I SEED ,  IPASS,  IPRRUN,  IPGSIZ,  IRUN,  IDATA, 
+  SEED 

COMMON  /  IF'OINT  /  IY,  IXS,  IXF,  IPF,  ISF,  IFIND,  IQIND,  IFF,  IQQ, 
+  IXSO,  IXFO ,  I PF 0 ,  IYDOT,  IXSDOT,  IXFDOT,  IPFDOT 

+  IDEWK,  IH,  ISTHT ,  IG,  ITKNOT ,  IUKNOT,  IUXTJ, 

+  ICOEF 

COMMON  /  NCOM  /  NF,  NS,  M,  NZF ,  NZQ ,  NXTJ,  NTR ,  NALL 
COMMON  /  OTHER  /  ZRES,  LXTJ,  IMEAS,  ALPHA 
LOGICAL  ENDFLG ,  MSFLG 
C 

C  ***  PROPAGATE  XS,  XF  AND  PF  BY  NUMERICAL  INTEGRATION  UNTIL 
C  ***  T-  AND/OR  TF  ARE  REACHED. 

C 


100  CONTINUE 

CALL  ADVANS  (T,  MSFLG,  ENDFLG) 

IF  (MSFLG)  THEN 

*#***#****##**##*###**#*****SOFE  MODIFICATION************************* 
***#*#***##*#*****###********SOFE  MODIFICATION************************ 
CALL  S0FM0D1 (NF,NXTJ,A( IXF) ,A( IDATA) ,A( IPF) , NTR ) 

y^f  y^  y^  y^  y^  y^,  y^  y^  y^  y^  y^  y|^  ^  ^  y|^  ^  y^y  ^  y|^  ^  ^  ^  f  ^  y^y  y|^  y^f  ^  y^  ^  ^  y^y  y|^  y^y  y^y  y^y  y^y  y^y  y^  y^y  y^y  y|^  y^y  y^y  y^y  y^y  y^y  y^y  y^y  y^y  y^ 

c 

C  ***  TIME  IS  T-  .  UPDATE  XF-  AND  PF-  .  BEGIN  BY  FINDING  SQUARE  ROOT 
C  ***  SF-  OF  PF-  .  THEN  SEQUENCE  THRU  M  MEASUREMENTS,  ONE  AT  A  TIME. 

C 

CALL  OUT  <  T ,  6) 

CALL  PSQRT  (NF,  NTR,  A(IPF>,  IERFLG ,  A(ISF)) 

IF  (IERFLG  .EG.  2)  CALL  ERROUT  (11,  'MCRUN') 

DO  110  II  =  1,  M 
IMEAS  =  II 

C 

C  ***  GET  MEASUREMENT  DATA  FROM  USER-ROUTINE  =HRZ=. 

C  ***  UPDATE  SF-  AND  XF-  IN  XSPLUS .  SQUARE  SF+  SO  PF  IS  CURRENT. 

C 


+ 

+ 


+ 

+ 


CALL  HRZ  (IRUN,  T,  NF,  NS,  NXTJ, 
A( IDATA) ,  NTR,  A(IPF), 
A ( IH ) ,  RMEAS ,  ZRES) 

IF  (RMEAS  .GE.  0.)  THEN 

CALL  XSPLUS  (NF,  NTR,  A(IH), 
A ( 1 ) ,  A< ISTHT ) , 
A(IXF),  A( ISF) ) 


A(IXF),  A ( IXS ) , 
IMEAS,  M, 


RMEAS,  ZRES, 

A ( IG ) ,  ALPHA, 


D-4 


,  **•  ,*  V. 


on  on 


SOFE  Modification 


♦DECK  S0FM0D1 

SUBROUTINE  SOFMODl ( NF , NXT J , AIXF , AIDATA , AIPF ,NTR) 

C 

C  +++  -SOFMODl-  REORIENTATES  THE  REFERENCE  FRAME  AT  TIME  T(I-l)  TO 
C  *•++  THAT  AT  TIME  T(I).  THIS  IS  ACCOMPLISHED  BY  DIRECTION  COSINE 
C  +++  MATRICES.  FIRST,  HEADING  AND  ROLL.  ANGLES  (EXTERNAL  TRAJECTORY 

C  +F+  VALUES >  AT  TIME  T(I)  ARE  DIFFERENCED  WITH  THOSE  AT  TIME  T(I-.1> 

C  +++  TO  FORM  DELTA  ANGLES  TO  USE  IN  THE  DIRECTION  COSINE  MATRICES. 

C  F++  THE  DIRECTION  COSINE  MATRIX,  ROTATE,  IS  THEN  APPLIED  TO  THE 
C  +++  FILTER'S  XF-  AND  PF-. 

C 

C  WRITTEN  BY  CAPT .  ROSS  ANDERSON 

C  SUBROUTINE  CALLS  CPYVM,  CPYMV,  MMUL ,  MTRN  WRITTEN  BY  OTHERS 

C 

COMMON/OL.D/TURNOLD ,  PHIOLD 
COMMON/ROTCOM/ROTATE (9,9) 

C  COMMON/MODI 'PMAT , WRKSPCX , WRKSPCP , AIXFM, ROTTRAN 

DIMENSION  AIXF (9) , AI DATA < 20 ) , AIPF ( 45 ) , PMAT ( 9 , 9 ) , 

1  WRKSF’CX (9,1), WRKSF'CP (9,9)  , AIXFM (9,1 )  , 

2  ROTTRAN (9,9) 

***  DETERMINE  DELTA  ROTATION  ANGLES  (INITIAL  VALUES  FROM  XFDOTO) 
DELTURN  =  A I DAT  A ( 19 )  -  TURNOLD 
DELPHI  =  A I DATA (20)  -  PHIOLD 
TURNOLD  =  A I DATA ( 19) 

PHIOLD  =  AIDATA(20) 

#**  FORM  DIRECTION  COSINE  MATRIX,  ROTATE ( SET=0 .  IN  SUBROUTINE  USRIN) 
ROT ATE (1,1)  =  COS (DELTURN) 

ROTATE (2,2)  =  R0TATE(1,1) 

ROTATE (3, 3)  =  ROTATE (1,1) 

ROTATE (1,4)  =  SIN (DELTURN) 

ROTATE (2, 5)  =  ROTATE  (1,4) 

ROT ATE (3,6)  =  ROTATE  (1,4) 

ROTATE (1,7)  =  0. 

ROT  ATE (2,8)  =  R0TATE(1,7) 

ROTATE (3, 9)  =  ROTATE (1,7) 

ROTATE  (4,1)  =  --COS ( DELPHI  )*SIN( DELTURN) 

ROTATE (5, 2)  ■  R0TATE(4,1) 

ROTATE ( 6,3 )  =  R0TATE(4,1) 

ROTATE <4, 4)  =  COS ( DELPHI )*COS( DELI  UN) 

ROTATE ( 5,5 )  =  R0TATE(4,4) 

R0TATE(6,6)  =  R0TATE(4,4) 

ROT  ATE (4,7)  =  SIN(DELPHI) 

R0TATE(5,8)  =  R0TATE(4,7) 

R0TATE(6,9)  =  R0TATE(4,7) 

ROTATE (7,1)  =  SIN (DELPHI )#SIN( DELTURN) 

ROTATE (8,2)  =  R0TATE(7,1) 

ROT  ATE (9,3)  =  R0TATE(7,1) 

ROT  ATE (7,4)  =  -SIN( DELPHI ) #COS ( DELTURN ) 

R0TATE(8,5)  =  R0TATE(7,4> 

R0TATE(9,6)  =  R0TATE(7,4) 

ROTATE (7, 7)  =  COS (DELPHI) 


no  r,  no  nnoo  on  no  noooo 


R0TATE<8,8)  =  R0TATE<7,7) 

R0TATE<?,9)  =  ROTATE (7,7) 

*#*  NOW  ROTATE  XF  (XF  =  ROTATE*XF) 

FIRST,  CONVERT  XF  INTO  A  COLUMN  VECTOR 

HO  50  1=1, NF 

AIXFM( 1,1 )=AIXF ( I ) 

50  CONTINUE 

SECOND,  MULTIPLY 

CALL  MMUL ( ROTATE , AIXFM , AIXFM , 

1  WRKSPCX , NF , NF , 1 ) 

FINALLY,  CONVERT  XF  BACK  INTO  A  ROW  VECTOR 
DO  60  1=1, NF 

AIXF< I )=AIXFM< 1,1) 

60  CONTINUE 

***  NOW  ROTATE  PF  (PF  =  ROTATE#PF* ( ROTATE  TRANSPOSED)) 

FIRST,  PF  MUST  BE  CONVERTED  FROM  A  VECTOR  TO  A  MATRIX 
CALL  CPYVM<NF,NTR,AIPF,PMAT) 

SECOND,  MULTIPLY,  F'MAT  =  ROTATE*PMAT 
CALL  MMUL < ROTATE, PMAT,PMAT, 

1  WRKSPCF' ,  NF  ,  NF  ,  NF ) 

THIRD,  MULTIPLY,  F'MAT  =  F'MAT*ROTTRAN  '  ROTTAN  =  ROTATION  TRANSPOSED 
CALL  MTRN(ROTATE,ROTTRAN,NF ,NF) 

CALL  MMUL  < F'MAT, ROTTRAN, F'MAT, 

1  WRKSPCF',  NF,NF,NF) 

FINALLY,  PMAT  MUST  BE  CONVERTED  FROM  A  MATRIX  BACK  TO  A  VECTOR 
CALL  CPYMV  < NF , NTR , PMAT , AIPF ) 

RETURN 

END 

♦DECK  CPYMV 

SUBROUTINE  CPYMV  <N,  L,  A,  R> 


SUBROUTINE  CPYMV 
PURPOSE 

COPY  THE  UPPER  TRIANGULAR  ELEMENTS  OF  A  SQUARE  MATRIX  INTO 
A  VECTOR.  TO  ILLUSTRATE,  THE  4X4  MATRIX  A  IS  COPIED  TO 
THE  10X1  VECTOR  R  AS  FOLLOWS. 

ABCD 

A  =  EFGH  ===»  R=(A,B,F,C,G,K,D,H,L,P> 

IJKL 

MNOP 


C  USAGE 

C  CALL  CPYMV(N,L,A,R) 

C 

C  DESCRIPTION  OF  PARAMETERS 

C  N  -  INPUT,  NUMBER  OF  ROWS  (COLUMNS)  IN  SQUARE  MATRIX  A 

C  L  -  INPUT,  NUMBER  OF  ELEMENTS  IN  VECTOR  R  <=N(N+l)/2) 

C  A  -  INPUT,  NAME  OF  INPUT  MATRIX 

C  R  -  OUTPUT,  NAME  OF  OUTPUT  VECTOR 

C 

C  REMARKS 

C  L  MUST  EQUAL  N(N+l)/2,  THE  NUMBER  OF  ELEMENTS  IN  THE 

C  UPPER  TRIANGULAR  PORTION  OF  A. 

C 

C  SUBPROGRAMS  REQUIRED 

C  NONE 

C 

C  . 

C 

DIMENSION  A ( N ,  N),  R(L) 

C 

K  =  0 

DO  110  J  =  1,  N 

DO  100  I  =  1,  J 
K  =  K  f  1 
R(K)  =  A ( I ,  J) 

100  CONTINUE 

110  CONTINUE 
RETURN 


END 

♦DECK  CPYVM 

SUBROUTINE  CPYVM  <N,  L,  R,  A) 


SUBROUTINE  CPYVM 


PURPOSE 

COPY  THE  ELEMENTS  OF  A  VECTOR  TO  FORM  A  SYMMETRIC  MATRIX 
TO  ILLUSTRATE,  THE  10X1  VECTOR  R  IS  COPIED  INTO  THE 
4X4  MATRIX  A  AS  FOLLOWS: 

ABDG 

R  =  (A,B,C,D,E,F,G,H,I,J)  ===»  A  =  BCEH 

DEFI 

GHIJ 


USAGE 

CALL  CPYVM(N,L,R,A) 


DESCRIPTION  OF  PARAMETERS 

N  -  INPUT,  NUMBER  OF  ROWS  (COLUMNS)  IN  SQUARE  MATRIX  A 
L  -  INPUT,  NUMBER  OF  ELEMENTS  IN  VECTOR  R  <=N(N+l>/2) 

R  -  INPUT,  NAME  OF  INPUT  VECTOR 


A  -  OUTPUT,  NAME  OF  OUTPUT  MATRIX 


REMARKS 

L  MUST  EQUAL  N<N+l>/2,  THE  NUMBER  OF  ELEMENTS  IN  THE 
UPPER  TRIANGULAR  PORTION  OF  A. 

SUBPROGRAMS  REQUIRED 


Equivalent  Discrete-Time  Algorithm 
PROGRAM  tXEC  ( SAS  Program)  6 

H  t-  OBJECTIVE  - 

-A  COMPUTER  AIDED  DESIGN  PACKAGE  TO  AID  IN  THE 
DEVELOPMENT,  DESIGN,  TUNING,  AND  EVALUATION  OF  AN  EXTENDED 
KALMAN  FILTER ( EKF )  FOR  AN  AIR-TO-AIR  FIRE  CONTROL  SYSTEM. 

THE  FILTER  IS  BASED  ON  A  LINEAR  DYNAMICS  MODEL  AND  NON¬ 
LINEAR  MEASUREMENTS.  A  CONSTANT  STATE  TRANSITION  MATRIX 
(STM)  IS  EMPLOYED  TO  PROVIDE  FILTER  PROPAGATION.  OVER¬ 
ALL,  THE  FILTER  PROVIDES  BODY  AXIS  REFERENCED  ESTIMATES  OF 
CARTESIAN  VARIABLES  OF  POSITION,  VELOCITY,  AND  ACCELERATION. 

-AN  EQUIVALENT  DISCRETE  TIME  DESIGN  OF  A  SPECIFIC  EKF 

-TO  BE  USED  TO  REPLACE  SOFE  AND  SOFEPL  FOR  A  SPECIFIC 
APPLICATION. 

-EKF  BASED  ON  A  NINE  STATE  REDUCED  ORDER  MODEL  OF 
POSITIONS),  VELOCITY  (3)  ,  AND  ACCELERATION  ( 3 )  . 

-DESIGNED  SO  APPLICABLE  PORTIONS  OF  SOFTWARE  CAN  BE 
'LIFTED  OUT'  FOR  EVENTUAL  IMPLEMENTATION  ON  THE 
,r-4  E/G  MODEL  AIRCRAFT. 

f  +  +  AUTHOR  -  CAPT  ROSS  ANDERSON 

-  *DECK  GAUSS  FROM  SOFE ( SLIGHTLY  MODIFIED) 

+  +  +  EXTERNAL  PROGRAMS  REQUIRED 

1.  HILLS  -  GENERATES  EXTERNAL  TRAJECTORY,  TRUTH  STATES, 

AND  MEASUREMENTS.  HILLS  IS 
RUN  PREVIOUS  TO  THIS  SIMULATION  AND  THE 
RESULTS  STORED  FDR  USE  BY  THIS  PROGRAM.  (DATA 
IS  STORED  IN  AN  UNFORMATTED  FORMAT) 


2.  NATHLIB 


•  MATRIX  ROUTINES  PACKAGE  WRITTEN 
BY  AFWAL/AAAN 


3.  POSTPROCESSOR  PLOTTING  PACKAGE  -  SUBROUTINE  P05TPRC 

GENERATES  THE  AVERAGE  OF  THE  DIFFERENCE  E  OF 
THE  TRUTH  STATE  AND  THE  FILTER  STATE,  THE 
STANDARD  DEVIATION  SE  OF  THIS  DIFFERENCE,  AND 
•  THE  PLUS  AND  MINUS  SQUARE  ROOT  VALUE  OF  THE 
DIAGONAL  OF  THE  FILTER  COMPUTED  COVARIANCE. 

A  POSTPROCESSOR  PLOTTING  PACKAGE  IS  REQUIRED 
TO  GRAPHICALLY  PLOT  THE  RESULTS,  BASICALLY, 

ANY  PLOTTING  PACKAGE  CAN  BE  USED.  FOR  PLOTS 
INCLUDED  IN  THESIS  WORK,  A  PLOT  M  FUNCTION 
FROM  A  PROC  FILE  CALLED  ’PROCFIL*  IS  USED. 

TO  GET  PROCFIL  ON  AN  AIR  FORCE  INSTITUTE  OF 
TECHNOLOGY <AFIT)  CYBER  ACCOUNT,  THE  FOLLOWING 
STEPS  MUST  BE  TAKEN: 

A.  LOGIN  TO  ACCOUNT 

B.  TYPE  IN  AFTER  THE  PROMPT  AND  THEN  •RETURN’ 
GET  ,F'R0CFIL/UN=T840115 

REPLACE, PROCFIL 
UPROC, PROCFIL 

BEGIN, PV, , XXX  (WHERE  XXX  IS  YOUR  USER 

JOB  NUMBER) 


n  o  o  o  o  non on  onononoonoo  nnnno  nnnnn  0000  nnnnnnn 


+++  external  functions  required 

1.  STANDARD  MATH  ROUTINES:  SORT ,  /,  *,  ETC. 

+  +  +  DEFINITION  OF  TERMS/VARI ABLES 

COMMON/INITPRB/NF,NS,M,NXTJ,T,TO,TF,DTMEAS,ISEED,IPASS,IRUN, 
1  IBUG1 t IBUG2  r IBUG3  > IBIJG4 , 1 BUGS  r I  BUGA  r I BUG7 

^  .  t  /  ■  I*  'I,  it/  il/  1  i>  .  1/  ^  i  1/  '  1/  »1«  ij.  t L/  il/  >|/  it/  •!/  ^  i b  it/  iL#  ^  ^  ^  ^  U/  ^ 

^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <T-  ^  ^  ^  ^  ^ 

****  INITIALIZE  PROBLEM  **** 

iL/  ^  ^  ^  ^  ^  ^  ^  ^  ^  iL/  ^  ^  ^  ^  ^  il/  ^  |b  ^  O/  ^  ^  1/  dr  ^  ^  dr  dr  dr 
rp  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  *  T 

CALL  INITPRB 

^  ^  ^  ^  i^»  i^/  ^  i^.  ^  ^  ^  ^  ^  >|r  >^r  ^  i^r  ^  V^r 

****  INITIALIZE  RUN  **** 

)f(  )|(  )fC  )((  l|(  )|(  ^  ^  ^  ^  ^  ^  \^r  j|r  t|j|  \^r  ^  <^r  ^  v^r  v^r  j^r  ^ 

100  CALL  INITRUN 

■  t.  jl/  yt(  ^  ^  ^  ^  ^  ^  ^  yb  1»  il  yL/  ^  ^  yb  yt,  yb  yb  yb  ib  b  tb  b  ^  b  b  \0  b  b  b  b  b  |b  b  b  b  b  b  b  b  b  b  b  b  b 

^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  *  *r  ^  ^  ^  ^  ^  ^  *  *  *  *  ^  *r  ^  ^  ^  ^  ^  ^ 

****  READ  EXTERNAL  DATA  FOR  CURRENT  TIME  **** 
XMMMMtMMtMXMttt************************ 

200  CALL  READEXT 

************************************************* 

****  GENERATE  NOISE  TO  ADD  TO  MEASUREMENTS  **** 
*******#******#**#r****************************** 

DONE  THROUGH  REAL  FUNCTION  GAUSS 

rp  ^  ^  ^  ^  ^  ^  ^  ^  r^  ^  ^  ^  ^  ^  ^  if.  /r  ^  ^  ^  ^  ^  ^  <P  *  “  ^  *p  T  ^  ^  “ 

****  KALMAN  FILTER  PROPAGATION  **** 

b  b  b  b  b  b  \U  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  \br  b  b  b  b  dr  b  b  b  b  b  b  b  b 

r^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  r^  ^  /p  rp  rp  ^  *p  *  *  *p  ^  ^  ^ 

CALL  PROPAG 

b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b 

****  KALMAN  FILTER  UPDATE  **** 

b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b 

^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  *  *  rp  *  ^  ^  ^  ^  ^  ^  ^  *  *  *  * 

CALL  UPDATE 

b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b 

^  ^  ^  ^  ^  ^  rp  ^  ^  ^  ^  ^  ^  ^  r^  ^  ^  ^  ^  ^  ^  ^  *  *  *  *  * 

****  SAVE  STATISTICS  **** 

b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b  b 

^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  r^  ^  ^  ^  ^  *  *  * 

CALL  SAVSTAT 
C 

t***************************'******************** 

#***  ANOTHER  TIME  PROPAGATION  AND  UPDATE?  **** 


noon  nnnnnnnon 


w  ^  Ilf  ^  ^  ^  ^  ^  ^  ^  ^  ^  vV  ^  ^  ^  vb  ^  «1»  t L#  ^  ^  \b  ^  \b  si*  >1>  'L>  i  i  i  \L*  '  L*  ^  ^  \b  vb  \b  *  1  >  '  L.  sb  i  'll  sL*  ■  Li 

*  ^r  ^  ^p  ^  *p  ^  *p  *p  ^  ip  ^  ip  *p  ip  i^  ^  ^  Ip  ip  ^  ^  Ip  ip  ^  *  ip  ^  ^  ^  ^  ^  1^  ^  ^  ^  ^  ^  ^ 

c 

IF  (T  .LT.  IF)  GO  TO  200 
C 

0  ^  ^  ^  >^i  ^  ^  ^  ^  i^i  ,^i  ^  ^  ^  i^i  ^i  ^  ^  j|^  ^  ^  >^i  ^  ^ 

C  ****  RUN  COMPLETE  #*** 

0  ^  ^  ^  S^f  S^f  ^  ^  s|^  ^  ^  ^  ^  V^f  ^  vj^  ^  '^1  ^  '^-  ^  ^ 

c 

IRUN  =  IRUN  +  1 

IF  (IRUN  .  LE ,  I  PASS )  GO  TO  100 
C 

C'L*  li  i  ab  '  1/  si  i  all  si  ^  dj  si  si  si  *  I*  si  ^  li  si  all  a li  si*  si  *i  a ll  si  li  Si  Si  i  '1 «  si  >i  ' If  dj  *b  d|  si  si  *1*  a Ll  da  all  si  i  si 

T  *  ^  ^  T  *  p  p  'f  ^  ^  ^  T  ^  ^  *  T  ®  p  p  *  *  T  p  p  T  Ap  ^  ^  ^  ^  ^  ^  T'  ^  ^  ^  ^  ^  ^  ip  i^  ^  ^ 

c  *#**  PROBLEM  COMPLETE  ,  POST  PROCESS  DATA  **** 

0  S^  s^  s^f  s^  sj^  s^  s|^  s^l  £  s^  s^f  s|^  s^s  ^  jy  *|^  s^l  s^|  sj^  s|^  ^  y^f  s^l  s^*  s|^  s^f  s|^  s^f  s^  ^  ^  a^#  y  s^j  ^  ^  i^i  sj^  sj^  s^l  ^*  s|^  ^|»  a^«  s^f 

c 

CALL  POSTPRC 
C 

STOP  '  PROBLEM  DONE ' 

END 

*  DECK  INITPRB 

SUBROUTINE  INITPRB 

+  +  +  =INITPRB=  IS  CALLED  ONCE  AT  THE  BEGINNING  OF  EACH  PROBLEM. 
+++  EXECUTION  TO  INITIALIZE  THE  PROBLEM.  INITPRB  SETS  PROBLEM 
+++  VARIABLES,  READS  PROBLEM  VARIABLES,  ECHOS  PROBLEM  VARIABLES, 
+++  CALCULATES  A  CONSTANT  STM  PHI,  CALCULATES 
+++  PROBLEM  STANDARD  DEVIATION,  SETS  THE  H  MATRIX  TO  ZERO. 

***  SETS  THE  ROTATE  MATRIX  TO  ZERO,  INITIALIZES 
***  SAVSTAT  VARIABLES,  AND  CALCULATES  A  CONSTANT  GD  MATRIX. 

COMMON/GFCOM./QFIN  ( 3 ) 

COMMON/QDCOM/QD ( 9 , ? ) 

COMMON/TAUCOM/TAU ( 3 ) 

COMMON /RFCOM/R MAT (6,6) 

COMMON/ROTCOM/ROTATE (9,9) 

COMMON/ SHE VCOM/STD ( 6 ) 

COMMON/ INITPRB/NF , NS , M , NXT  J , T , TO , TF , DTMEAS , I SEED , IPASS , IRUN , 
1  IBUG1 , IBUG2 , IBUG3 , IBUG4 , I  BUGS , IBUG& , I BUG 7 

COMMON/SAVST  AT /XE ( 9 ) , SUMXE ( 500 ,9,2) , SUMPPL ( 500 , 9 ) 

COMMON/PHI COM/PHI (9,9) 

COMMON/HCOM/H (6,9) 

COMMON/ INPCTRL/NTSIN 
DIMENSION  RF I N ( 6 ) 

REAL  H 

EQUIVALENCE ( OUTDAT , OUTBEAM , OUTTAIL ) 

***  SETUP  INPUT  AND  OUTPUT  TAPES 


TAPE3 

TAPE4 

TAPES 


INPUT 

OUTPUT 

INPUT 


UNFORMATTED 

UNFORMATTED 

FORMATTED 


-EXTERNAL  TRAJECTORY  DATA 
-OUTPUT  THAT  CAN  BE 
POSTPROCESSED 
-TITLES,  R,  Q,  TAU,  AND 
INITIALIZATION  PARAMETERS 


c 

c 

c 


TAPEA 


OUTPUT 


FORMATTED 


I PASS  AND  I BUG 
-ECHO  PROGRAM  AND  I  BUG  DATA 


0PEN<UNIT=3,FILE='0UTDAT'  , STATUS= ' OLD '  , FORM= ' UNFORMATTED '  ) 
OPEN ( UNI T=4,FILE=/PLTDATA' , STATUS= ' NEW ' r FORM  = ' UNFORMATTED '  ) 
0PEN(UNIT=5,FILE='SADATA' , ST  ATUS  = ' OLD ' ) 
OPEN(UNIT=A,FILE='ECHO' ,STATUS='NEU' ) 

C 

C  ***  SET  PROBLEM  VALUES 
C 

NF=9 
NS  =  9 
M=A 

NXT  J=20 
T0=0  . 

TF=12 .0 
DTMEAS= . 1 
IPASS=20 
I SEED=77 
IRUN=1 
I BUG 1=1 
IBUG2=1 
IBUG3=1 
IBUG4= 1 
IBUG5=1 
IBUGA=1 
IBUG7=1 
NTS.IN=5 
C 

C  ***  NOTE  THE  ABOVE  I BUG  PARAMETERS  CONTROL  OUTPUT 
C  TO  TAPE A.  NTS IN  CONTROLS  THE  NUMBER  OF  TIME 

C  STEPS  READ  FROM  THE  EXTERNAL  TRAJECTORY  TAPE, 

C  EXAMPLE,  IF  NTSIN=5,  THE  FIRST  EXTERNAL  DATA 

C  RECORD  IS  READ  THEN  EVERY  FIFTH  ONE  THEREAFTER. 

C  NTS  MUST  BE  COORDINATED  WITH  DTMEAS  OF  THIS 

C  PROGRAM  AND  DT  OF  PROGRAM  HILL.5 

C 

C  ***  READ  NAMELIST  VARIABLE  PROBLEM  VALUES 
G 

NAMELIST/PRBINIT/QF IN , RF IN , TAU , IPASS , IBUG1 , 

1  I BUG2 , 1 BUG 3 , 1 BUG 4 , IBUG5 , 1  BUGA , IBUG7 , 

2  NTSIN , TF 
READ ( 5 , PRBIN IT ) 

C 

C  ***  RMAT ( M , M )  TO  ZERO 

C 

DO  30  I = 1 , M 

DO  20  J= 1 , M 
RMAT ( I , J ) =0 , 

20  CONTINUE 
30  CONTINUE 
C 

C  ***  FILL  IN  NONZERO  RMAT  VALUES 


c 


RMAT<1 , 1 ) =RFIN ( 1 ) 

RMAT (2,2) -RFIN (  2 ) 

RMAT <3,3)=RFIN(3) 

RMAT(4,4)=RFIN(4) 

RMAT(5,5)=RFIN(5) 

RMAT  (6,6)  --RFIN  <  6 ) 

C 

c 

C  ***  ECHO  PROBLEM  VALUES 

WRITE (6,#) 7*****THE  FOLLOWING  ARE  SET  IN  SUB  INITPRB  AS  t  ***** 7 
WRITE<6,*> 

C 

WRITE ( £ ,  *) 7 NF= 7  ,  NF 
WRITE! A,*) 7NS=7 , NS 
WRITE < 6,*) 7M=7 ,M 
WRITE (A,*) 7 NXT  J= 7 ,NXTJ 
WRITE  ( i_-  r  * )  '  TO= 7  ,  TO 
WRITE ( 6 , #  > 7  TF= 7 , TF 
WRITE ( 6 , # ) 7 DTMEAS= 7 , DTMEAS 
WRITE(6,*) 7ISEED=7 , ISEED 
WRITE ( 6 , * ) 7  IRUN= 7  ,  IRIJN 
WRITE ( 6 ,#> 7  IPASS= '  ,  IF’ASS 
WRITE (A.*) 7G<1)=7 ,QFIN(1) 

WRITE  (A,*)  7Q<2)=7  ,QFIN(2) 

WRITE < A,*) 7 0(3)=' ,QFIN(3) 

WRITE  <  £ , * ) 7  RFIN ( 1 ) - 7 , RFIN ( 1 ) 

WRITE (A,*) 7RFIN<2)~7 ,RFIN(2) 

WRITE(A,*> 7RFIN<3>*7 fRFIN(3) 

WRITE (6,#) 7RFIN(4>*7 , RFIN! 4) 

WRITE (A,*) 7RFIN!5)»7 , RFIN! 5) 

WRITE ( £ , # ) 7  RFIN ( A ) - 7 , RFIN ! 6 ) 

WRITE (A,*) 7TAU!1)~7,  TAU ( 1 ) 

WR I TE ( A , * ) 7  TAU ( 2 )  = 7 , TAU ( 2 ) 

WR I TE ( A , # ) 7  T AU ( 3 )  = 7 , TAU ( 3 ) 

WRITE!6,*>  7 IBUG1=7  ,  IBIJG1 
WRITE! A»*) 7 IBUG2= 7  > IBUG2 
WRITE  !  A  ,  * )  7  IBUG3=  7  , 1  BUG  3 
WRITE (A,*) 7 IBUG4=7 , IBUG4 
WRITE! A,*) 7 IBUG5- 7 , I  BUGS 
WRITE ! £ , # )  7 1  BUGA- 7  ,  IBIJGA 
WRITE  (A,*)  'IBUG7®7  ,IBUG7 
WRITE ! £ , #  > 

C 

C  ***  CALCULATE  CONSTANT  STATE  TRANSITION  MATRIX (STM) .  IN 
C  ***  REALITY,  THIS  MATRIX  CAN  BE  PRECOMPUTED  FOR  A  CONSTANT 
C  ***  UPDATE  RATE  (DTMEAS).  THEN,  ONLY  THE  PRECOMPUTED  VALUES 
C  ***  HAVE  TO  BE  STORED.  CALCULATIONS  ARE  INCLUDED  HERE  FOR 
C  ***  FLEXIBILITY,  IE,  IT  IS  VERY  EASY  TO  CHANGE  TAU  AND 
C  **#  DTMEAS. 

C 

C  SET  ALL  VALUES  OF  STM  PHI  TO  ZERO 
C 


E-5 


o  n  n  n  n  n  non  non 


DO  50  1=1, NF 

DO  40  J=1,NF 
PHI < I , J ) =0 . 

40  CONTINUE 
50  CONTINUE 

CALCULATE  NON-ZERO  VALUES  OF  PHI. 

PHI < 1 , 1 ) = 1 . 

PHI ( 1 ,2)=DTMEAS 

PHI  (1 ,3)=TAU<  1  )**2*<  <  1  ./TAUC 1 )  )  *DTMEAS-1 .  +EXF* <  -  <  ( 1  ,/TAU(  1 )  >* 
1  DTMEAS ) ) ) 

PHI (2,2>  =  1 . 

PHI <  2 , 3) =TAU ( 1 ) # ( 1 . -EXP ( - ( ( 1 . /TAU ( 1 ) > *DTMEAS ) ) ) 

PHI (3,3>=EXP(-< ( 1 ./TAU< 1) )#DTMEAS> ) 

PHI <  4 , 4) =1 . 

PHI (4,5) =DTMEAS 

PHI <4,6)=TAU<2)**2*<  < 1 ./TAU<2) ) #DTMEAS-1 . +EXP< -( ( 1 . /TAU ( 2 ) ) * 
1  DTMEAS))) 

PHI ( 5 , 5 ) =1 » 

PHI <  5 , 6 ) =TAU ( 2 ) #  < 1 . -EXP  < - < < 1 . /TAU < 2 ) ) *DTMEAS ) ) ) 

PHI  (6,6)  =EXF‘ <  - <  ( 1  ./TAU<2)  >*DTMEAS)  ) 

PHI <  7 , 7 ) =1 . 

PHI ( 7 , 8 ) =DTMEAS 

PHI ( 7 , ? ) =TAU ( 3 ) **2* ( ( 1 . /TAU ( 3 ) ) *DTMEAS-1 . +EXP ( - C ( 1 . /TAU ( 3 ) )  * 
1  DTMEAS))) 

PHI <  3 , 8 ) -1 . 

PHI (8,9) =T AU ( 3 ) *  < 1 . -EXP ( - ( ( 1 . /TAU ( 3 ) ) *DTMEAS  > ) ) 

PHI <  9 , 9 ) =EXP ( - ( ( 1 . /TAU  <  3 ) ) *DTMEAS ) ) 

***  CALCULATE  STANDARD  DEVIATION 

STD ( 1 )  =  SORT ( RFTN ( 1 ) ) 

STD  <  2 )  =  SORT <  RFIN ( 2 ) ) 

S T D ( 3 )  =  SORT  <  RFIN<  3 ) ) 

STD ( 4 )  =  SORT ( RF IN  <  4 ) ) 

STD ( 5 )  =  SORT ( RFIN <  5 ) ) 

8  T  D  <  6 )  =  SORT  <  RF IN  <  6 ) ) 

***  SET  ALL  VALUES  OF  THE  H,  GD,  AND  ROTATE  MATRICES  TO  ZERO 

DO  60  1=1, NF 
DO  55  J=1 ,NF 
IF  (I  .LE.  M)  THEN 
H( I , J) =0 . 

ENDIF 

ROTATE ( I , J)=0. 

GD ( 1 , J ) =0 . 

55  CONTINUE 
60  CONTINUE 

*#*  SET  INITIAL  VALUES  OF  POSTPRC  TO  ZERO 


■.* l* 1  *  i1.1 11.*  *'  'j  »'v  v'  '.■i?'  ■■ 


->  *j*  ■  ■- 


•>  ■> '  1  -rr'T r^» LVTAT^ “V 


k- 

Is! 


* 


fv 


DO  65  1=1,500 

DO  64  J=1 , NF 

SUMXE (I,J,1)=0. 

SUMXE ( I , J , 2 ) =0 . 

SUMF'F'L  ( I » J>  =0. 

64  CONTINUE 

65  CONTINUE 
C 

C  ♦  ♦♦  CALCULATE  GD. 

C  ♦  ♦♦  THE  EXACT  VALUE  OF  GD  IS  CALCULATED.  IN  REALITY,  GD 
C  ♦♦♦  CAN  BE  PRECOMPUTED  AND  VALUES  STORED  FOR  IMPLEMENTATION 
C  ♦  ♦♦  ON  A  SYSTEM.  CALCULATIONS  ARE  INCLUDED  HERE  FOR 
C  ***  FLEXIBILITY,  IE,  IT  IS  VERY  EASY  TO  CHANGE  VALUES  OF 
C  ♦  ♦♦  TAU,  GFIN,  AND  DTMEAS . 

C 

GD< 1 ,1>  =  <  <GFIN<l>*TAU<l)**5)/2. ) * ( 1 . -EXP< - ( 2 . ♦DTMEAS/TAIJ ( 1 ) ) )  + 

1  2. ♦DTMEAS/TAU ( 1 ) +2 . *DTMEAS##3/ < 3 . *TAU < 1 > **3> - 

2  2 . *DTMEAS**2/TAU ( 1 ) **2- ( 4 . *DTMEAS/TAU ( 1 )  )  ♦ 

3  EXP <  ~(.OTMEAS/TAU( 1 ) ) ) ) 

GD  ( 1 , 2 )  =  ( <  GFIN ( 1 ) *TAU ( 1 ) **4 ) /2 . ) * ( 1 . +EXP < - <  2 . *DTMEAS/TAU ( 1 )  )  )  - 

1  2 .  ♦EXF"  (  -  ( DTMEAS /TAU <  1  > )  )  +  ( 2  .  *DTMEAS/TAU<  1  >  >* 

2  EXP  ( -  ( DTMEAS/T AU  ( 1 ) ) ) -2 .  ♦DTMEAS/TAU  ( 1 )  + 

3  DTMEAS**2/TAIJ<  1)**2> 

GD ( 1 , 3 )  =  ( ( GFIN ( 1 ) *TAU  < 1 ) **3 ) /2 . ) ♦ < 1 . -EXP ( - ( 2 . ♦DTMEAS/TAU ( 1  > )  >  - 
1  ( 2 . ♦  EiTMEAS/T AU < 1 ) ) ♦EXP ( - ( DTMEAS/TAU ( 1 > ) )  ) 

UD<2,2)  =  <  <GFIN(1  >*TAU<1  >#*3>/2.  >  *  <  4  .  *EXP  (  -  <  DTMEAS/TAIJ  ( 1 ) ) >-3.- 
1  EXP ( - <  2 . ♦DTMEAS/T  AU ( 1 ) ) ) +2 . ♦DTMEAS/TAU ( 1 ) ) 

GD  ( 2 , 3 )  =  <  <  OF  IN  ( 1  >  ♦TAU  ( 1 )  ♦« )  /2 .  )  ♦  <  1 .  +EXP  ( -  ( 2 .  ♦DTMEAS/TAU  <  1 )  )  )  - 
1  2  » ♦EXF^  - ( DTMEAS/TAU< 1 ) > ) ) 

GD  (  3 , 3 )  =  ( GFIN  ( 1  >  ♦TAIJ  ( 1 )  /2 .  >  ♦  ( 1  ♦  -EXP<  -  ( 2  .  ♦DTMEAS/T AIJ  ( 1 )  )  )  > 

GD  (2,1)  =GD  (.1,2) 

QD<3, 1 )=GD< 1,3) 

GD  (3,2)  --GD  (2,3) 

GD ( 4 , 4 )  =  (  ( GF T. N  (  2  )  ♦T AU  (  2  ) ♦♦S )  /2  .  )♦(  1 .  -EXP  (-<  2 .  ♦DTMEAS/TAU  <  2 )  )  )  (• 

1  2 . ♦DTMEAS/TAU ( 2 ) +2 . ♦  DTMEAS^3/ ( 3 . ♦TAU ( 2 ) **3 ) - 

2  2 . ♦DTMEAS^^2/TAU ( 2 ) ♦  ♦2- ( 4 . ♦DTMEAS/TAU ( 2 ) ) ♦ 

3  EXP(- (DTMEAS/TAU (2) >) > 

GD ( 4 , 5 ) = ( ( GFIN ( 2 ) ♦TAU( 2 ) ♦  iM ) /2 . )♦( 1 . +EXP (-( 2 . ♦DTMEAS/TAU ( 2 ) ) >- 

1  2. ♦EXP (-(DTMEAS/TAU (2) ) ) P( 2 , ♦DTMEAS/TAU ( 2) )♦ 

2  EXP( -(DTMEAS/TAU (2)  )) -2 .  ♦DTMEAS/TAIJ ( 2 )  + 

3  DTMEASW2/TAU(2)^2) 

GD ( 4 , 6 ) = ( ( GFIN ( 2 ) ♦TAU ( 2 ) ♦♦S ) /2 . )♦( 1 . -EXP (-( 2 . ♦DTMEAS/TAU ( 2 ) ) )- 
1  (  2  .  ♦DTMEAS/T AIJ  (  2 )  )  ♦EXP  (  -  (  DTMEAS/TAU  (  2 ) )  )  ) 

GD ( 5 , 5 )  =  ( ( GFIN ( 2 ) ♦TAU ( 2 ) ♦♦S ) /2 , )♦( 4 . ♦EXP (-( DTMEAS/TAU ( 2 )  )  )-3.- 
1  EXP ( - <  2 » ♦DTMEAS/TAU ( 2 ) ) ) +2 . ♦DTMEAS/TAU ( 2) ) 

GD ( 5 , 6 )  =  (  (GFIN  ( 2 ) ♦T AU (  2  )  W2 )  /2 »  )  ♦  ( 1  ,  +EXP ( -  (  2  .  ♦DTMEAS/T AU  (  2 )  )  )- 
1  2.^EXP( -(DTMEAS/TAU(2>  > ) ) 

GD(6,6)  =  (GFIN(2)^TAU(2)/2.  )♦(  1 . -EXF’( -( 2 .  ♦DTMEAS/TAU  ( 2 )  )  )  ) 

GD (5,4) =QD (4,5) 

GD (6,4) =QD( 4,6) 

GD ( 6 , 5 ) =QD (5,6) 

GD ( 7 , 7 )  =  <  ( GFIN (  3 )  ♦TAU ( 3 )  ♦♦S) /2  .)♦(!.  -EXP (  -  ( 2 » ♦DTMEAS/TAIJ (  3 ) )  )  + 
1  2 .  ♦DTMEAS/TAIJ  (  3 )  +2  .  ♦DTMEAS^^3/  (  3  .  ♦TAU  ( 3 )  )  - 


E-7 


n  n  n  n 


2  2  .  *DTMEAS**2/TAU  <  3 )  **2~  <  A  .  *DTMEAS/TAU  (  3 )  )  * 

3  EXP ( - ( DTMEAS/T AIJ <  3 ) ) ) ) 

QD!7,8>=! !RFIN!3)*TAU!3)**4)/2. >*!1 . f EXP ! - ! 2 . *DTMEAS/TAU < 3 ) ) ) 

1  2. *EXPt-! DTMEAS/TAU! 3>  >)  +  ! 2. *DTMEAS/TAU! 3) >* 

2  EXP  ( -  ( DTMEAS/TAU  (3)  ) ) -2  .  *DTMEAS/TAU!  3 )  + 

3  DTMEAS**2/TAU!3)**2> 

QD!7,9)=! !RFIN!3)*TAU!3>**3>/2. )*(1 ,-EXP!-!2.*DTMEAS/TAU!  3) ) ) 
1  !2.#DTMEAS/TAU! 3) ) *EXP <-( DTMEAS/TAU  ( 3 ) ) ) ) 

RD ! 8 , 8 ) - !  !RFIN!3)*TAU!3)**3>/2.  )  * !  4  .  *EXP !  ~  !  DTMEAS/TfiU !  3 )  )  )  -3  . 
1  EXP ! - ( 2 . ♦ DTMEAS/TAU ( 3 ) ) )+2. #DTMEAS/TAU  <  3 )  ) 

RD !  8 , 9 )  = ! !RFIN!3)*TAlJ!3)**2>/2. >*!  1 . +EXP ! - ! 2 . *DTMEAS/TAU ! 3 ) ) ) 
1  2.*EXP!~!DTMEAS/TAU!3> ) ) ) 

RD(9,9)=!QFIN!3)*TAU!3)/2. >*! 1 . -EXP ! - ! 2 , *DTMEAS/TAU ! 3 > ) ) ) 

RD ! 8 , 7 ) =RD !  7 , 3  ) 

QD!9,7)=QD!7,9> 

QD ! 9 , 8 ) -QD  !  8 , 9 ) 

WRITE  <  6 ,  #  > 

WRI TE ! 6 , * ) ' PHI  MATRIX 7 
WRITE !  6 ,  * ) 

DO  70  K  =  1 ,  NF 

WRITE! A,  .100)  !K,  I  ,PHI  !K,  I )  ,1  =  1  ,NF> 

70  CONTINUE 
WRITE !  6 ,  # ) 

WRITE ! 6,* ) 'RD  MATRIX' 

WRITE !  6 ,  * ) 

DO  75  K= 1 , NF 

WRITE!6,100>!K,I,QD!K,I) ,I=1,NF> 

75  CONTINUE 
WRITE! 6 f#) 

C 

RETURN 

100  FORMAT !9!It,Il,E11.5,lX) ) 

END 

♦  DECK  INITRIJN 

SUBROUTINE  INIIRUN 
C 

C  +++  -INITRUN*  IS  CALLED  ONCE  AT  THE  BEGINNING  OF  EACH  RUN  TO  SET 
C  +++  UP  THE  RUN.  FIRST,  INITIAL  VALUES  OF  PPLUS  ARE 
C  +++  SET.  THEN,  THE  HEADER 

C  +  +  +  INFORMATION  IS  STRIPPED  OFF  THE  XTRAJ  TAPE  <  T APE3 ) . 

C  +  +  +  THIS  TAPE  INFORMATION  IS  ECHOED  TO  TAPE  &  IF  IRUN=1, 

C  +++  ICOUNT  IS  SET  EQUAL  TO  0  FOR  USE  IN  SAVSTAT . 

C 

COMMON/I NITPRB/NF , NS , M , NXT J , T , TO , TF , DTMEAS , I SEED , IPASS , I RUN , 

1  I  BUG  1 , 1 BUG2 , 1 BIJG3 , 1 BUG4 , 1  BUGS ,  I BUG6 ,  IBUG7 

COMMON/UPDATE/ZRES ( 6,1 ) ,XFPLUS(9,1 ), PPLUS (9, 9) 

COMMON/ INITRUN/ ICOUNT 
CHARACTER  TITLE*40,M0DE*20,NAME*30 

**#  INITIALIZE  PO(PPLUS)  FOR  EACH  PASS! IPASS) 

***  SET  P  VALUES  ERUAL  TO  ZERO 


DO  80  1  =  1  *  NF 

DO  70  J=1 , NF 
PF'LUS  ( I » J ) =0  . 
CONTINUE 
CONTINUE 

SET  NON-ZERO  P  VALUES 


PF’LUS  ( 1 , 1  >  = 
F'F'LUS  ( 2 » 2  >  = 
F'F'LUS  (3 , 3>  = 
PF’LUS  (4,4)= 
F’F'LUS  <  5,5)  = 
F'F’LUS  (6, 6  )  = 
PF’LUS  (7,7)  = 
F'F’LUS  (8,8)  = 
F'F’LUS  (9,9)  = 


=1000. 
=  4000. 
=9300. 
=1000. 
=  4000. 
=9300. 
=  1000. 
=  4000. 
=9300. 


READ  EXTERNAL  TRAJECTORY  TO  POSITION  POINTER  ON  DATA. 


1000 


♦  DECK 


REWIND  3 
READ ( 3 )  TITLE 
READ (3)  NAME 
READ ( 3 )  MODE 
IF  (IRUN  .EG.  1)  THEN 
WRITE ( 6 , 1000 )  TITLE 
WRITE ( 4 )  TITLE 
WRITE (6, 1000)  NAME 
WRITE ( 4 )  NAME 
WRITE (6, 1000)  MODE 
WRITE ( 4 )  MODE 
END  IF 
I COUNT =0 
RETURN 
FORMAT (A40) 

END 

READEXT 

SUBROUTINE  READEXT 


C 

C  *** 


•  =READEXT=  IS  CALLED  FOR  EMERY  RUN  FOR  EMERY  TIME  VALUE 

■  STORED  ON  THE  EXTERNAL  TRAJECTORY  OUTPUT  OF  HILLS 
-  UP  TO  TIME  TF. 

•  FILE  OUTDAT  IS  READ  FOR  XTRAJ  STORED  DATA. 

■  THE  EXTERNAL  TAPE  TIME  MUST  EQUAL  OR  EXCEED  TF . 

COMMON/  INI  TPRB/NF ,  NS ,  M ,  NXTJ ,  T ,  TO ,  TF ,  DTMEAS ,  I  SEED ,  IF’ASS ,  IRUN 
1  IBUG1 , IBUG2, IBUG3, IBUG4, IBUG5, IBUG6, IBUG7 

COMMON/READEXT/XTRA J ( 20 ) 

COMMON/INPCTRL./NTSIN 
COMMON/ JUNK/X JUNK ( 20 ) 

c  READ  TIME  AND  NXTJ  EXTERNAL  DATA  VALUES 
READ ( 3 )  T,(XTRAJ(I), 1=1, NXTJ) 

INCNTRL=NTSIN-1 


•  ■  »  • 


)  rr.v>v- 

)  0.  i  v' 


DO  90  J=1 ,  INCNTRL. 

READ ( 3 )  T JUNK  , ( X JUNK ( I ) , 1  =  1 ,NXTJ) 

90  CONTINUE 
RETURN 
END 
C 

♦DECK  GAUSS 

REAL  FUNCTION  GAUSS  (MEAN,  STD) 

C 

C  . . . . .  It 

C 

C  REAL  FUNCTION  GAUSS 

C 

C  PURPOSE 

C  SAMPLE  A  GAUSSIAN  (NORMAL)  DISTRIBUTION  WHOSE  MEAN  AND 

C  STANDARD  DEVIATION  ARE  GIVEN 

C 

C  USAGE 

C  SAMPLE  =  GAUSS (ME AN, STD) 

C 

C  DESCRIPTION  OF  PARAMETERS 

C  MEAN  -  DESIRED  MEAN  OF  THE  GAUSSIAN  DISTRIBUTION 

C  STD  -  DESIRED  STANDARD  DEVIATION  OF  THAT  DISTRIBUTION 

C  GAUSS  -  THE  VALUE  OF  THE  COMPUTED  GAUSSIAN  RANDOM  VARIABLE 

C 

C  SUBPROGRAMS  REQUIRED 

C  PORTABLE  RANDOM  NUMBER  GENERATOR  SELECTED: 

C  NONE 

C  NON-PORTABLE  RANDOM  NUMBER  GENERATOR  SELECTED: 

C  RANF(D) 

C  UNIFORM  RANDOM  NUMBER  GENERATOR  FOR  CDC  EQUIPMENT. 

C  PRODUCES  SAMPLES  IN  THE  INTERVAL  (0,1). 

C  RANSET(N) 

C  THE  SEED  INITIALIZATION  ROUTINE  ON  THE  CDC. 

C 

C  REMARKS 

C  TWO  RANDOM  NUMBER  GENERATORS  ARE  PROVIDED,  ONE  COMPLETELY 

C  PORTABLE,  THE  OTHER  NON-PORTABLE  (CDC  SPECIFIC). 

C  THE  PORTABLE  GENERATOR  PRODUCES  THE  SAME  RANDOM  NUMBER 

C  SEQUENCE  (TO  32  BITS)  ON  ALL  MACHINES. 

C  THE  USER  CHOOSES  BETWEEN  THEM  BY  HIS  INITIAL  ASSIGNMENT 

C  OF  ISEED ,  THE  SEED  USED  TO  GENERATE  UNIFORM  DEVIATES. 

C  ISEED  IS  INPUT  TO  GAUSS  THROUGH  COMMON/INITPRB/ . 

C  IF  ISEED  IS  INITIALLY  POSITIVE,  A  PORTABLE  GENERATOR  IS 

C  SELECTED. 

C  IF  ISEED  IS  NEGATIVE,  A  NON-PORTABLE  GENERATOR  IS  SELECTED 

C  BASED  ON  CDC ' S  RANF  AND  RANSET  FUNCTIONS. 

C  IF  ISEED=0,  GAUSS  ALWAYS  RETURNS  ZERO. 

C 

C  THIS  METHOD  HAS  BEEN  SHOWN  TO  PRODUCE  RELIABLY  NORMAL 

C  DEVIATES  EVEN  IN  THE  TAILS  OF  THE  DISTRIBUTION. 

C  ON  THE  AVERAGE  THERE  ARE  1.37746  UNIFORM  RANDOM  NUMBERS 

C  NEEDED  FOR  EACH  CALL  OF  GAUSS,  CONSIDERABLY  FEWER  THAN 


E-10 


/vVy 


I* 


C  THE  NUMBER  REQUIRED  BY  OTHER  METHODS,  INCLUDING  THE  MORE 

C  COMMONLY  USED  TWELVE-SUM  APPROACH. 

C 

C  METHOD 

C  REFERENCE:  RICHARD  p.  BRENT,  *A  GAUSSIAN  pseudo-random 

C  NUMBER  GENERATOR*,  THE  COMMUNICATIONS  OF  THE  ACM, 

C  DECEMBER  1974. 

C 

C  THE  METHOD  USED  WAS  SUGGESTED  BY  VON  NEUMANN,  AND 

C  IMPROVED  BY  FORSYTHE,  AHRENS,  DIETER  AND  BRENT. 

C 

C 

C  STEP  1  IF  THE  FIRST  CALL,  TAKE  A  SAMPLE  U  FROM  THE  UNIFORM 

C  DISTRIBUTION  ON  (0,1),  OTHERWISE  U  HAS  BEEN  SAVED 

C  FROM  A  PREVIOUS  CALL. 

C 

C  STEP  2  SET  X  =  A(I)  +  < A( 1+1 )-A( I ) )*U  AND  U(0)=G(X). 

C 

C  STEP  3  TAKE  INDEPENDENT  SAMPLES  U ( 1 ) ,U( 2 ) , . . . .  FROM 

C  THE  UNIFORM  DISTRIBUTION  ON  (0,1)  UNTIL  FOR  SOME 

C  K.GE.l,  U(K-l) .LE.U(K) . 

C 

C  STEP  4  SET  U  =  (U(K)-U(K-l ) )/< 1 .-U<K-1 ) ) 

C 

C  STEP  5  IF  K  IS  EVEN  GO  BACK  TO  STEP  2,  OTHERWISE  DETERMINE 

C  SIGN  FOR  X,  ADJUST  X  TO  MATCH  THE  DESIRED  MEAN  AND 

C  STANDARD  DEVIATION,  AND  RETURN  X,  THE  NORMAL  DEVIATE. 

C 

L 

C 

C 

C  WARNINGS 

C  DIMENSION  AND  DATA  STATEMENTS  BELOW  ARE  MACHINE  DEPENDENT. 

C  THE  DIMENSION  OF  D  MUST  BE  AT  LEAST  THE  NUMBER  OF  BITS  IN  THE 

C  FRACTION  OF  A  FLOATING-POINT  NUMBER.  THUS,  ON  MOST  MACHINES, 

THE  DATA  STATEMENT  BELOW,  AND  THE  SIZE  OF  D  CAN  BE  TRUNCATED. 
U  MUST  BE  PRESERVED  BETWEEN  CALLS. 


COMMON  /  ICOM  /  ICONT,  ISEED,  IF'ASS,  IPRRUN,  IF'GSIZ,  I  RUN,  IDA  TA 

SEED 

COMMON/ 1  NITF'RB/NF ,  NS,  M ,  NXTJ,T ,  TO,  TF ,  DTME  AS ,  ISEED ,  IF'ASS ,  I  RUN . 

IBUG1 , IBUG2 , I BUG3 , IBUG4 , I BUGS , IBUG6 , I BUG? 

DIMENSION  DC 60) 

REAL  MEAN 

DATA  D  /  0.674489750,  0.475859630,  0.383771164,  0.328611323, 
0.291142827,  0.263684322,  0.242508452,  0.225567444, 
0.211634166,  0.199924267,  0.189910758,  0,181225181, 
0.173601400,  0.166841909,  0.160796729,  0.155349717, 
0.150409384,  0.145902577,  0.141770033,  0.137963174, 
0,134441762,  0.131172150,  0.128125965,  0 . 1 252790?0 , 
0.122610883,  0.120103560,  0.117741707,  0.115511392, 


n  n  o 


+ 

+ 

+ 

+ 

+ 

+ 

+ 

+ 


0 . 1 1340234? , 
0.105976772, 
0.099827234, 
0.094627461 , 
0.090156838, 
0.086260215, 
0.082824924, 
0.079766932, 


0.111402720, 
0.104334841  , 
0.098448282, 
0.093448407, 
0.089133866, 
0.085361834, 
0.082027847, 
0.079053527, 


0 .109503852 , 
0.102766012, 
0.097124309, 
0.092311909, 
0.088144619, 
0.034490706, 
0.081253162, 
0.073353731 , 


0.107697617, 

0.101265052, 

0.095851778, 

0.091215432, 

0.087187293, 

0.033645437, 

0.080499844, 

0.077681899 


***  END  OF  MACHINE-DEPENDENT  STATEMENTS 


DATA  U  /  0.  / 

DATA  ISELCT  71/ 


IF 

(STD  .LT. 

0.  ) 

ISELCT  =  1 

A  = 

0. 

I  = 

0 

GO 

TO  (  100, 

140, 

140,  190  ),  ISELCT 

C 

C  ***  STEP  1  AND  INITIALIZATION: 

C  ***  SET  ISELCT  SO  SUBSEQUENT  CALLS  GO  DIRECTLY  TO  THE  APPROPRIATE 
C  ***  GENERATOR:  I.E.  IF  ISEED<0,  =0  OR  >0,  SELECT  THE  NON-PORTABLE 
C  ***  ZERO  PRODUCING,  OR  PORTABLE  GENERATORS,  RESPECTIVELY. 

C  ***  IF  THE  CDC  NON-PORTABLE  CASE  IS  SELECTED  (ISEEDCO), 

C  ***  INITIALIZE  THIS  GENERATOR  USING  ' RANSET '  f  FOR  THE  PORTABLE 
C  ***  GENERATOR,  INITIALIZE  THE  CONSTANTS  THAT  ARE  USED. 

C 

100  IF  ( I SEED )  110,  120,  130 
C 

110  ISELCT  =  3 

CALL  RANSET  (I SEED) 

IJ  =  RANFO 


U 

=  U  ) 

-  tJ 

IF 

(U 

.GE 

GO 

TO 

140 

120 

ISELCT 

==  4 

GO 

TO 

190 

130 

ISELCT 

-i 

DATA  C7P5  /  16807,  /  ,  C2P31M  /  2147433647.  /  ,  C2P31  / 
f  2147483648.  / 

SEED  =  ISEED 

SEED  =  AM0D(C7P5*SEED,  C2P31M) 

U  =  SEED  /  C2P31 
U  =  U  +  (J 

IF  (U  .GE.  1 . )  U  =  U  -  1. 

C 

C  *#*  INCREMENT  COUNTER  I  AND  DISPLACEMENT  U  IF  LEADING  BIT  OF  U  IS 
C 

140  LJ  =  U  +  tJ 

IF  (U  .LT.  1.)  GO  TO  150 
U  =  U  -  1.0 
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1  =  1  +  1 
A  =  A  +  0  (  I  ) 

GO  TO  140 
C 

C  ***  STEP  2:  NOTE  THAT  U  DENOTES  UNIFORM  SAMPLES  U(K),  WHERE  K  IS 
C  ***  V  DENOTES  THOSE  WHERE  K  IS  EVEN. 

C  ***  FORM  W  UNIFORM  ON  0.  LE .  W  .  L.T .  0(1  +  1)  FROM  U. 

C  ***  W(I+1)=(A(I+1>-A(I))*U 
C 

150  U  =  IK  I  +  1)*IJ 
C 

C  ***  SET  V=  U ( 0 ) =G ( X  )  WHERE  G(X)=.5*(X**2-(A( I ) )**2> 

C 

V  =  W*<0.5*W  +  A) 

C 

C  ***  STEP  3:  GENERATE  NEW  UNIFORM  U. 

C  ***  IF  PORTABLE  GENERATOR  HAS  BEEN  SELECTED,  UNIFORM  SAMPLES 
C  ***  U(I)  ARE  COMPUTED  FROM  THIS  RECURRENCE  RELATION: 

C  ***  U(J.  +  1)  =  7**5  U<I)  MODULO ( 2**31  -  1) 

C  ***  IF  NONPORTABLE  GENERATOR  HAS  BEEN  SELECTED,  RANF  PROVIDES 
C  ***  UNIFORM  SAMPLES, 

C 

160  IF  ( I3ELCT  ,  EQ  .  2)  SEED  =  AMOD  ( C7F'5*SEED ,  C2P31M) 

IF  ( ISELCT  .EQ.  2)  U  =  SEED  /  C2P31 

IF  (ISELCT  .EQ.  3)  U  =  RANFO 

C 

C  ***  ACCEPT  W  AS  A  RANDOM  SAMPLE  IF  V  .LE.  U. 

C 

IF  (V  »LE .  U)  GO  TO  170 
C 

C  ***  GENERATE  NEW  RANDOM  V. 

C 

IF  (ISELCT  , EQ .  2)  SEED  =  AMOD ( C7P5*SEED ,  C2P31M) 

IF  (ISELCT  .EQ,  2)  V  =  SEED  /  C2P31 

IF  (ISELCT  .EQ.  3)  V  =  RANFO 

C 

C  ***  CONTINUE  TAKING  UNIFORM  RANDOM  SAMPLES  UNTIL  U  .LE.  V. 

C 

IF  (U  ,GT,  V)  GO  TO  160 
C 

C  ***  STEPS  4  AND  5  FOR  K  EVEN  ’  REJECT  W  AND  FORM  A  NEW 
C  ***  UNIFORM  U  FROM  U  AND  V.  GO  BACK  TO  STEF'  2. 

C 

IJ  =  ( V  -  IJ )  /  ( 1  .  -  U ) 

GO  TO  150 
C 

C  ***  STEPS  4  AND  5  FOR  K  ODD:  FORM  NEW  UNIFORM  U  FROM  U  AND  V 
C  ***  (TO  BE  USED  IN  NEXT  CALL). 

C 

170  U  =  (IJ  -  V)  /  ( 1  .  -  V) 

C 

C  ***  USING  FIRST  BIT  OF  U  TO  DETERMINE  SIGN,  RETURN  NORMAL  DEVI  ATI: 


A0-A1C4  034  PRELIHINARV  KALHAN  FILTER  DESIGN  TO  IMPROVE  AIR  COHORT 
MANEUVERING  TARGET.  .  <U>  AIR  FORCE  INST  OF  TECH 
MRIGHT-PATTERSON  RFB  OH  SCHOOL  OF  ENGI.  .  R  B  ANDERSON 
UNCLASSIFIED  DEC  85  RFIT/GE/ENG/85D-2  F/G  19/5 


3/5 

NL 


V'r^' 


vw  W,  *.■  i*  rrcTT 


u  =  u  +  u 


IF  <U  .LT. 
U  =  U  -  1. 

1 .  )  GO 

TO 

180 

GAUSS  =  (W 
RETURN 

+  A)*STD 

+ 

MEAN 

ISO  GAUSS  =  - 

(W  +  A)*STD 

+  MEAN 

RETURN 

c 

C  ***  GENERATE  ALL  ZEROS 
C 

190  GAUSS  =  0.0 
RETURN 
END 

*DECK  F'ROPAG 

SUBROUTINE  PROPAG 
C 

C  +  +  +  =PR0PAG=  IS  CALI  ED  FOR  EVERY  RUN  FOR  EVERY  TIME  VALUE 
C  F  +  +  STORED  ON  THE  EXTERNAL  TRAJECTORY  OUTPUT  OF  HILLS. 

C  H+  PROPAG  EMPLOYS  A  CONSTANT  STATE  TRANSITION  MATRIX 
C  T++  CALCULATED  IN  SUBROUTINE  INITPRB  TO  PERFORM  THE  EXTENDED 
C  +++  KALMAN  FILTER  PROPAGATION.  THIS  IS  POSSIBLE  BECAUSE  OF 
C  +  +  +  THE  LINEAR  DYNAMICS  MODEL.  THE  EQUATIONS  EMPLOYED  ARE  t 
*#*****#**#**#***#**#*******##*#*****#*#**#***#*******##***#**# 

X 

C  +  +  +  (EQ  1)  XF(TI-)  =  ROT  ATECPHI*XF (TI-1+>T  +  BD*U 

C  +++  <EQ  2)  P ( T I - )  =  R0TATE*CPHI*P(TI-1F)*PHIT]*R0TTRAN  +  QD 

* 

XXXXXXXXXXXMXX'MXXXXXXXtXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX 

r*******r*  **********#*******#######**###**####*###*#*##*#*#*** 

C  +++  WHERE  ROTATE  AND  ROTTRAN  ARE  NECESSARY  TO  ROTATE  XF(TI-1+) 

C  H-  +  AND  P ( T I  - 1  + )  INTO  THE  SAME  FRAME  AT  TIME  (TI-).  THIS  IS 
C  +++  NECESSARY  BECAUSE  HILLS  (WITH  OPTION  3  SELECTED)  GENEATES 
C  F  +  +  STATES  AND  MEASUREMENTS  REFERENCED  TO  A  RADAR  REFERENCE 
C  +++  FRAME  (A  BODY  FRAME)  WHICH  CAN  CHANGE  DURING  THE  PROPAGATION. 
C  Ul  NOTE  THAT  THE  BD*U  TERM  OF  EQ  1  IS  NOT  ROTATED.  THIS  IS 
C  +++  BECAUSE  THE  BD*U  TERM  IS  REFERENCED  TO  THE  ROTATING  FRAME. 

C  +++  NOTE  THAT  QD  IS  NOT  ROTATED  WITH  THE  REST  OF  EQ  2. 

C  +++  THIS  IS  BECAUSE  QD  IS  REFERENCED  TO  THE  ROTATING  FRAME. 

C  +++  IF  I BUG 1=1  THE  ROTATE  MATRIX  IS  ECHOED  TO  TAPE6  FOR  IRUN-l. 

C  +  +  +  IF  I  BUG 2=1  THE  XF(TI-)  VECTOR  IS  ECHOED  TO  TAPE6  FOR  IRUN=1 . 

C  +++  IF  I BUG3= 1  THE  P(TI-)  MATRIX  IS  ECHOED  TO  TAPE 6  FOR  IRUN=1 . 

C 

COMMON/INITPRB/NF,NS,M,NXTJ,T,TO,TF,DTMEAS,ISEEDf IPASS, IRUN, 

1  I BUG 1 , I BUG2 , IBUG3 , I BUG4 , I BUGS , IBUG6 , I BUG7 

C0MM0N/PHIC0M/PHI<9,9) 

C0MM0N/UPDATE/ZRES<6,1> ,XFPLUS(9, 1) ,PPLUS<9,9) 
C0MM0N/PR0PA/XFF(9,1)  ,P<9,9)  ,F’MINUS<9,9)  f  XFMINUS  <  9 , 1 ) 
COMMON/QDCOM/QD (9*9) 

COMMON./READEXT/XTRAJ<20) 

COMMON/ROTCOM/ROT ATE  (9*9) 

COMMON/ QFCOM/QFIN( 3) 
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DIMENSION  WRKSF'CX  (9,1 )  ,WRKSPCF‘ ( 9 , 9  ) 

***  IF  TIME  IS  ZERO  INITIALIZE  TURNOLD  AND  PHHIOLD 

IF  (T  . EQ •  0.)  THEN 
TURNOLD=XTRAJ< 19) 

F'HHIOLD=XTRA  J  (  20) 

XFPLUS  < 1 ,  1)=XTRAJ(7) 

XFF'LUS(2,1)=XTRAJ(8) 

XFPLUS(3,1)=XTRAJ(9) 

XFPLUS (4,1) =XTRAJ  < 10 ) 

XFPLUS (5,1) =XTRAJ (11) 

XFPLUS ( A , 1 ) =XTRAJ( 12) 

XFPLUS (7, 1 )=XTRAJ( 13) 

XFPLUS (8,1) =XTRAJ ( 14 ) 

XFFLUS(9, 1 )=XTRAJ( 15) 

RETURN 

ENDIF 

***  ROTATE  REORIENTATES  THE  REFERENCE  FRAME  AT  TIME  T(I-l)  TO 
**#  THAT  AT  TIME  T(I).  THIS  IS  ACCOMPLISHED  BY  DIRECTION  COSINE 
***  MATRICES.  FIRST,  HEADING  AND  ROLL  ANGLES  (EXTERNAL  TRAJECTORY 
***  VALUES)  AT  TIME  T(I)  ARE  DIFFERENCED  WITH  THOSE  AT  TIME  T(I-l) 
***  TO  FORM  DELTA  ANGLES  TO  USE  IN  THE  DIRECTION  COSINE  MATRICES. 
***  THE  DIRECTION  COSINE  MATRIX,  ROTATE,  IS  THEN  APPLIED  TO  THE 
***  FILTER'S  XF-  AND  PF-. 

***  DETERMINE  DELTA  ROTATION  ANGLES 
DELTURN  *  XTRA J ( 19 )  -  TURNOLD 
DELPHHI  =  XTRA J ( 20 )  -  PHHIOLD 
TURNOLD  =  XTRA J (19) 

PHHIOLD  =  XTRA J ( 20 ) 

***  FORM  DIRECTION  COSINE  MATRIX,  ROTATE 
ROTATE (1,1)  =  COS (DELTURN) 

ROTATE (2,2)  =  ROTATE (1,1) 

ROTATE (3,3)  =  R0TATE(1,1) 

ROT  ATE (1,4)  -  SIN ( DELTURN ) 

ROTATE (2, 5)  =  ROTATE  (1,4) 

ROTATE (3, 6)  =  ROTATE  (1,4) 

ROTATE( 1,7)  =  0. 

ROT  ATE (2,8)  =  R0TATE(1,7> 

ROTATE (3, 9)  =  R0TATE(1,7) 

ROT  ATE (4,1)  =  -COS<  DELPHHI )#SIN(DELTURN> 

ROTATE (5,2)  =  R0TATE(4,1) 

ROTATE (6,3)  =  ROTATE (4,1) 

ROTATE ( 4,4)  =  COS(DELPHHI > *COS < DELTURN) 

ROTATE (5, 5)  =  ROTATE (4, 4) 

ROT  ATE (6,6)  =  R0TATE(4,4) 

ROTATE (4, 7)  =  SIN (DELPHHI) 

ROT  ATE (5,8)  =  R0TATE(4,7) 

ROTATE (6, 9)  =  ROTATE (4, 7) 

ROT  ATE (7,1)  =  SIN ( DELPHHI )#SIN( DELTURN) 


ono  n -  onn  noonrsono  oonn 


ROT  ATE (8,2)  =  R0TATE(7,1) 

ROT  ATE  (9,3)  =  R0TATE(7,1) 

ROTATE (7,4)  =  -SIN ( DELPHHI ) *COS ( DELTURN ) 

ROT  ATE (8,5)  =  RQTATE(7,4) 

ROTATE ( 9,6)  =  R0TATE(7,4) 

ROT  ATE  (7,7)  =  COS  ( DELPHHI  ) 

ROT  ATE (8*3)  =  R0TATE(7,7) 

ROTATE (9,9)  =  ROTATE <7, 7) 

***  NOW  PERFORM  EG  1  MATRIX  MULTIPLICATION  CALCULATIONS 
***  X(TI-)=XFMINUS 

CALL  MMUL <  PHI , XFPLUS , XFF , URKSPCX , NF , NF , 1 ) 

CALL  MMUL (ROTATE,XFF, XFMINUS ,WRKSPCX , NF , NF , 1 ) 

*#*  NOW  ADD  EQ1  DU  TERM  TO  GET  XFMINUS 

SINCE  VFX ( OR  XTRAJ ( 16 )  )  TS  THE  ONLY  NONZERO  BD*U  TERM 
SUBTRACT  BD*VFX  FROM  XFMINUS(1,1) 

XFMINUS ( 1 , 1 )=XFMINUS(1 , 1 ) -DTMEAS*XTRAJ ( 16 ) 

***  NOW  PERFORM  EG  2  CALCULATIONS,  P(TI-)=PMINUS 
FIRST  TERM  (RIGHT  HAND  TERM  BEFORE  +  S[GN) 

CALL  MABT ( PPLUS , PHI , P , WRKSPCP , NF , NF , NF ) 

CALL  MMUL (PH I ,P,P, WRKSPCP, NF,NF,NF> 

CALL  MABT ( P , ROTATE , P , WRKSPCP , NF , NF , NF ) 

CALL  MMUL ( ROTATE , P , P , WRKSPCP , NF , NF , NF ) 

NOW  ALLOW  FOR  ■  ADAPTIVE  TYPE*  CHANGES 

IF  (T  . GE .  3.00  .AND.  T  .LT.  3.10)  THEN 
QFTN(l) -37325. 

GFIN(2>=37325. 

QF IN  •'  3 )  =3  7325 , 

CALL  ADAPOD 
END  [F 

CF(T  .GE.  6.00  .AND.  T  .LT.  6.10)  THFN 
QFIN( 1) *373250. 

QF[N(2)=373250. 

QFIN(3)=373250. 

CALL  ADAPGD 
ENDIF 

NOW  ADD  GD  TO  P  TO  GET  P( TI-)=PMINUS 
CALL  MADD ( P ,QD , PMINUS ,9,9) 

***  TAPE 6  OUTPUT 

WRITE<6,*) 

WRITE ( 6 , * ) ' #:M***#*******#**#***:M*#***#**#***##***#**#****#*# 
WRITE (6 . # )  '  '  ,  T ,  '  #**#######**####***#** 

IF  ( TBUG1  .EG.  1  .AND.  IRUN  .EG.  1)  THEN 
WRITE (6,*) 


W  '  1  5  %  V  V  V*  T1 


■  v  ",  -r  v 


'■w  w’l' 


V.V.- 

■to 


“•v-v* 


& 


WRITE  <  6 ,  HO ‘ ROTATE  MATRIX  AT  TIME  -  ' ,T 
WRITE (6,*) 

DO  110  K  =  1 ,  N  F 

WRITE ( 6 ,  200  M  K » I f ROTATE ( K » I ) , 1=1 ,NF) 

110  CONTINUE 

WRITE  <  A ,  HO 
ENDIF 

IF  (IBUG2  .EQ.  1  .AND.  IRUN  .EO.  1)  THEN 
WRITE ( 6 ,  HO 

WRITE(6,») /XF(TI->  OR  XFMINUS  VECTOR  AT  TIME  =  ',T 
WRITE ( 6 ,  HO 

WRITE (6, 210)  (I, XFMINUS (1,1)  ,  I=1,.>F) 

WRITE (6fl) 

ENDIF 

IF  (IBUG3  .EQ.  1  .AND.  IRUN  .EQ.  1)  THEN 
WRITE( A,*) 

WRITE ( 6 ,  HO  ' P ( TI- )  OR  PMINUS  MATRIX  AT  TIME  =  '  ,T 
WRITE  ( 6 ,  HO 
DO  120  K=1,NF 

WRITE (6, 200) <  N ,  I , PMINUS ( K ,  I ) , 1=1 ,NF> 

CONTINUE 
WRITE  <  A ,  HO 
ENDIF 
RETURN 

FORMAT  (?( II  ,H  ,E11 .5,  IX)  > 

FORMAT (?< IX, II, Ell. 5, IX )) 

END 

He  DECK  ADAPOD 

SUBROUTINE  ADAPQD 

=ADAFQD=  ALLOWS  FOR  FORCING  ADAPTIVE  CHANGES  IN  Q  BASED 


120 


200 

210 


+++ 

+++ 

+++ 


k  •  m  m  m 
- 

i.  *  » 


.  V 
.■  V,‘.‘ 


ON  LOOKING  AT  PREVIOUS  PLOTS  AND  Q  TO  OBTAIN  DESIRED 
PERFORMANCE. 

COMMON/QFCOM/QFIN ( 3 ) 

COMMON/QDCOM/QD (9,9) 

COMMON/ T  AUCOM/T  AU<  3 ) 

COMMON/ INI TF'RB/NF ,NS,M,NXTJ,T ,TO,TF ,DTMEAS,  ISEED,IPASS,  IRUN, 

1  IBUG1 , IBUG2 , IBUG3 , IBUG4 , I BUGS , IBUG6 , IBUG7 

QD < 1 , 1 )  =  <  < OF I N < 1 ) *T AU < 1 > #*5 ) /2 . > *  < 1 .  -EXP  ( -  ( 2 .  *DTMEAS/TAU  ( 1 )  ) )  + 

1  2.*DTMEAS/TAU< 1 > +2 . *DTMEAS#*3/ ( 3 . *TAU< 1>**3>- 

2  2 .  HtDTMEAS#H<2/T  AU(l)Ht*2-(4.  #DTMEAS/TAU(  1 )  )  ♦ 

3  EXP( - ( DTMEAS/T AU( 1 ) >  > ) 

QD(1,2)  =  (  <QFIN<l)*TAU<l>**4>/2.  > * < 1 . +EXP< - < 2 . *DTMEAS/TAU< 1 > >  )- 

1  2 .  #EXP <  - <  DTMEAS/TAU ( 1  )>>■§■<  2 .  *DTMEAS/TAU ( 1 )  ) * 

2  EXP( -( DTMEAS/TAU ( 1 ) >  >-2.*DTMEAS/TAU< 1 >  + 

3  DTMEAS**2/TAU<1>**2> 

QD  < 1 , 3 )  =  ( <  QFIN  < 1 ) *TAU  < 1 ) **3 ) /2 . ) * ( 1 . -EXP ( - <  2 . #DTMEAS/TAU ( 1 ) ) ) - 
l  ( 2 . HDTMEAS/T  AU< 1 > ) *EXP ( - ( DTMEAS/T  AU( 1 > ) ) ) 

QD  <  2 , 2 ) ■ < ( QFIN  < 1 ) *TAU  < 1 >  #*3 ) /2 . ) * ( 4 . #EXP ( - <  DTMEAS/TAU ( 1 ) ) ) -3 . - 
1  EXP ( - ( 2 . HDTMEAS/TAU ( 1 ) ) ) +2 . #DTMEAS/TAU< 1 ) ) 

QD  <  2 , 3  )  =  <  <  OF  IN  <  1 )  HtTAU  <  1 )  **2  >  /2 .  >  *  <  1  •  +EXP  ( -  <  2 .  *DTME AS/TAU  ( 1  >  )  >  - 
1  2 . #EXP  < - <  DTMEAS/TAU ( 1 ) > ) > 

QD(3,3>ss<QFIN<l)#TAU(l)/2.)Ht(l,  -EXP<  -  ( 2 .  *DTMEAS/TAU(  1 )  ) )  ) 


E"17  -  - 


DO* 


QD(2,1)=QD<1,2) 

QIK3, 1  )=QD<  1,3) 

QD ( 3 , 2 ) -QD (2,3) 

QD ( 4 , 4 ) - ( <QFIN(2)*TAU(2)**5)/2. ) * ( 1 . -EXP ( - ( 2 . *DTMEAS/TAU< 2 ) ) )+ 

1  2.*DTMEAS/TAU(2)+2.*DTMEAS**3/<3.*TAU(2)**3)- 

2  2 . *DTMEAS**2/TAU ( 2 >  **2- ( 4 . *DTMEAS/T  AU ( 2 ) ) * 

3  EXP (  -  ( DTMEAS/TAU  <  2 ) ) > ) 

QD < 4 , 5 ) --■  <  <QFIN(2)*TAU(2)*#4>/2.  >*(  1  .  +EXP < - ( 2 . *DTMEAS/TAU<  2  )  )  >- 

1  2  .  *EXP  <  -  (  DTMEAS/TAU  (  2 )  >  )  +  ( 2  .  *DTMEAS/TAll  <  2 )  >* 

2  EXP ( - ( DTMEAS/TAU ( 2 ) ) > -2 . *DTMEAS/TAU ( 2 ) + 

3  DTMEAS**2/TAU(2)**2> 

QD ( 4 , 6  >  ~ ( <QFIN<2)*TAU<2>**3>/2. )*<1 . -EXP  < -<  2.  #DTMEAS/TAIJ(  2 ) ) )- 
1  ( 2  » *DTMEAS/TAU ( 2 ) ) *£XP (-( DTMEAS/TAU ( 2 ) ) ) ) 

QD( 5 , 5 ) ~ ( ( QFIN ( 2 ) *TAU ( 2 ) **3 ) /2 . ) * ( 4 . *EXP ( - ( DTMEAS/TAU <  2 ) )  > -3 . - 
1  EXP( -<2,*DTMEAS/TAU(2) ) ) f 2 . *DTMEAS/TAU ( 2 ) ) 

QD( 5 , 6 ) = ( <QFIN<2)*TAU(2>**2>/2. >*( 1 . ¥  EXP (-( 2 , ^DTMEAS/TAU ( 2 ) ) ) -■ 
l  2 ,  *EXP(  -  ( DTMEAS/T AIJ  ( 2 )  )  )  ) 

QD (  6 , 6 )  =  < QFIN ( 2 ) *TAU( 2 )/2 • >*( 1 . -EXP < - ( 2 . *DTMEAS/TAU ( 2 ) ) ) ) 
QD(5,4>=QD(4,5> 

QD  <6,4 )  =QD (4,6) 

QD ( 6 , 5 ) -QD (5,6) 

QD ( 7 , 7 ) = ( (QFIN(3)*TAU(3)**5>/2. ) * ( 1 , - EXP ( - ( 2 . *DTMEAS/TAU ( 3 ) ) )+ 

1  2.*DTMEAS/TAU(3)T2.*DTMEAS**3/(3.*TAU(3>**3) - 

2  2,*DTMEAS*#2/TAU(3)**2-<4.*DTMEAS/TAU(3) >* 

3  EXP ( - ( DTMEAS/TAU ( 3) ) ) ) 

QD ( 7 , 8 ) = ( (QFIN(3)*TAU(3)**4)/2. ) * ( 1 . +EXP ( - ( 2 . *DTMEAS/TAU ( 3 ) ) )- 

1  2 . *EXP < - ( DTMEAS/TAU  (  3 ) ) >  +  <  2 . *DTMEAS/TAU  <  3 ) ) * 

2  EXP(-(  DTMEAS/T  All  (3)  )  ) -2  ♦  *DTMEAS/TAU  ( 3 )  + 

3  DTMEAS**2/TAU(3)#*2> 

QD(7,9)=(  <QFIN(3)*TAU(3)**3>/2.  >*(1  . -EXP  (  -  ( 2 .  *DTMEAS/TAIJ  (  3  >  )  )- 
1  ( 2. *DTMEAS/TAU( 3) )*EXP(~( DTMEAS/TAU (3) ) ) ) 

QD ( 8 , 8 ) - ( (QFIN(3)*TAU(3>**3)/2. >*( 4 , *EXP (-( DTMEAS/TAU ( 3 ) > >-3.- 
1  EXP(-(2.*DTMEAS/TAU(3) ) ) +2 , *DTMEAS/TAU( 3 ) ) 

QD(8,9)=( (GFIN(3)*TAU(3>**2>/2. )*( 1 . +EXP ( - ( 2 . *DTMEAS/TAU ( 3 > ) )- 
1  2.*EXP< -( DTMEAS/TAU ( 3) )> ) 

QD(9,9)  =  (QF  iN(3)*TAU(3)/2. ) * ( 1 . -EXP ( - ( 2 . *DTMEAS/TAU ( 3 ) ) ) ) 

QD ( 8 , 7 ) =QD (7,8) 

QD (9,7) =  QD<  7 , 9 ) 

QD ( 9 , 8 ) =QD (3,9) 

RETURN 

END 

♦DECK  UPDATE 

SUBROUTINE  UPDATE 
C 

C  +++  “UPDATE®  IS  CALLED  FOR  EVERY  RUN  FOR  EVERY  TIME  VALUE 
C  ¥+¥  STORED  ON  THE  EXTERNAL  TRAJECTORY  OUTPUT  OF  HILL5. 

C  +++  UPDATE  INCORPORATES  MEASUREMENT  INFORMATION  INTO  STATE 
C  *F+  AND  COVARIANCE  ESTIMATES  ACCORDING  TO  THE  FOLLOWING » 

******************************************************************** 


+++  (EQ  3) 
+++  (EQ  4) 


K(TI)  =  P(TI-)*HT*INVC(H(XF(TI-))*P(TI-)*HT)  +  R3 
XF ( T 1+ )  =  XF(TI-)  +  N(TI)*CZM  -  ZHM3 


*•  n 


■  J  r.  *>_ 


+  +  +  (EQ  5)  P(TI+>  =  P(TI-)  -  CK  (  T I )  *H  ( XF  ( TI  - )  )  *p<  TI- )  !l 


*********************************************  ***##*###*#####**  ****** 
******************************************************************** 

C  +++  WHERE  :  ZM  IS  THE  ’ACTUAL  MEASUREMENT  REALIZATION.’  NOISE 
C  +++  IS  ADDED  THROUGH  REAL  FUNCTION  GAUSS. 

C  +++  J  ZHM  IS  H*X(TI-)  OR  THE  KF  ESTIMATE  OF  THE  MEASUREMENT 

C  +  F+  :  H ( XF <  T I - ) )  IS  THE  PARTIAL  DERIVATIVE  EVALUATION  OF 

C  ff+  THE  H  MATRIX 

C  +++  IF  IBUG4-1  THE  H  MATRIX  IS  ECHOED  TO  TAPE6  FOR  IRUN=1. 

C  +  +  +  IF  IE«UG5=1  THE  K<TI)  VECTOR  IS  ECHOED  TO  TAPEA  FOR  IRIJN-1. 

C  +++  IF  I BUGA =1  THE  X(TI+)  VECTOR  AND  P(TI+)  MATRIX  IS  ECHOED  TO  TAPE 

C  +++  FOR  IRUN=1. 

C 

COMMON/ 1 NITPRB/NF , NS , M , NXT  J , T , TO , TF , DTMEAS , ISEED . IPASS , IRON  , 

1  IBUG1 , I BUG2 , I BUG3 , IBUG4 , I BUG5 , I BUGA , I BUG7 

COMMON/UPDATE/ZRES <  A , 1) , XFPLUS < 9  , 1 ) , PPLUS (9,9) 

COMMON/RFCOM/RMAT (  A  ,  A  ) 

COMMON/PROPA/XFF (9,1),P(9,9> , PMINUS ( 9 , 9 ) , XFMINUS ( 9 , 1 ) 
COMMON/SDEVCOM/STD <  A ) 

COMMON/READEXT /XTRA J  <  20 ) 

COMMON/HCOM/H  (6,9) 

DIMENSION  TEMPI <  A , 9 ) , TEMP2 (6,6) , TEMPK (9,6) , WRKSPC1 ( A , 9 ) , 

1  WRKSPC2  < 6,6), WRKSPC3 ( 9 , A ) , WRNSPC4 (9,1), 

2  WRNSPC5 (9,9) , WRKVEC1 ( A) ,URNVEC2(6) 

REAL  X r ( 9 ) , H 

C 

C  ***  IF  TIME  IS  ZERO  RETURN 
C 


IF  (T  .  E  Q  •  0.)  RETURN 
C 

C  * k*  FIRST,  CHANGE  FROM  DOUBLE  TO  SINGLE  SUBSCRIPT 
C 

XF  (  l  >=XFMINIJS<  1 , 1  > 

XF  (  2 )  :=XF  MINUS  (2,1) 

XF  (  X )  •--XFMINUS  (  3 , 1 ) 

XF  <  4 ) = XFMINUS (4,1) 

XF ( 5 )= XFMINUS (5,1 ) 

XF (6)=XFMINUS(6,1) 

XF (? )=XFMINUS(7, 1 ) 

XF(3)=XFMINUS(S, 1 ) 

XF (?)=XFMINUS(9 , 1  ) 

C 

C  ***  NOW  CALCULATE  6*9  H  MATRIX 
C 

C  CALCULATE  SOME  INTERIM  VALUES  FOR  EFFICIENCY 

C 

SUMSQ  =  XF(1)#*2  +  XF ( 4 ) **2  +  XF(7)#*2 
SUMS012=  SORT (SUMSQ) 

SUMSQ32=  (SUMSQ) **1.5 
SUMSQ2  =  ( SUMSQ ) **2 
PSIJM  =  XF  ( 1 )  **2  +  XF  ( 4  )  **2 
PSUM12  -  SQRT(PSUM) 


E-19 


Vv 


n  n  n  o 


PSUM2  =  PSUM**2 
PSIJM32  =  PSUM**1.5 

ADD1  =  <XF<1)*<XF(2)-XTRAJ<16))HXF<4)#XF(5)  +  XF( 
ADD2  =  XF <  4 ) -  XF<1)#*2 

A HD 3  =  XF<8)*PSUM  -  XF  <  7  >  *  <  XF  <  1  >  *  <  XF  <  2  >  -  XTRAJ(  16  >  ) 

L  XF  <  4 ) *XF  <  5 ) ) 

CONTINUE  CALCULATING  H 


XF ( 7 ) #XF ( 8 ) 


H  <  1 , 1 ) 
H( 1 ,4) 
H  <  1 , 7  > 
H  <  2 , 1 ) 
H<2,2) 
H  <  2 , 4  ) 
H  <  2 , 5 ) 
H  (  2 , 7  ) 
H  <  2 , 8 ) 
H  ( 3 , 1 ) 
H  (  3  f  4  ) 
H  <  4 , 1  ) 
H  (  4 , 4  > 
H<  4 , 7 ) 
H  <  5  »  1  ) 

H<5,2) 

H(5,4) 

H  <  5 , 5  ) 
H<6, 1 ) 


H  ( 6 , 2 ) 
H  <  6 , 4  ) 

H  <  6 , 5 ) 
H  <  6 , 7 ) 


XF ( 1 ) /SUMSQ1 2 
XF ( 4 ) /SUMSG12 
XF ( 7)/SUMSQ12 

<  <XF<2)-XTRA.J(  16)  )*SUMSQ  -  XF <  1 ) *ADD1 ) /SUMSQ32 
H(  1 , 1 ) 

<XF(5)*SUMSQ  -  XF(4)*ADD1 )/SUMSQ32 


H  <  6  r  8  )  = 


=  ( XF  (  5 )  4SUMSQ  -  XF  <  4  )  *ARD1 ) /SUMSQ32 
=  H  <  1 , 4 ) 

=  <XF<8)*SUMSQ  -  XF ( 7 ) #ADD1 ) /SUNSQ32 
=  H  ( 1 , 7 ) 

=  -XF  (  4  )  /F'SUM 
=  XF ( 1 ) /PSUM 

=  <XF< 1 )*XF<7) )/<SUMSQ*PSUM12) 

=  <XF<4)*XF<7) )/<3UMSQ#PSUM12) 

=  -PSUM1 2/SUMSG 

=  <  XF ( 5 ) *ADD2  +  2.*XF< 1 > * ( XF ( 2 ) -XTRAJ( 16) )*XF<4) ) 
/PSUM2 
=  H<3,1) 

=  ( (XF(2)-XTRAJ(16) )*ADD2  -  2 . *XF ( 1 ) *XF ( 4 ) *XF ( 5 ) ) 
/PSUM2 

-  H<3,4) 

=-< <SUMSQ*PSUM*<2.*XF<1)*XF<8>  - 
<XF<2)-XTRAJ< 16) )*XFC7> > >  -  ADD3* 

XF(1)*<2,*PSUM  +  SUMSQ) > / < SUMSQ2*PSUM32 ) 

-  H  (  4 , 1 ) 

=  -<  <SUMSQ*P<5'JM*(2.*XF<4)*XF<3>  -  XF  ( 5 )  *XF  (  7 )  )  )  -  ADD 3* 
XF<4)*>  SUM  +  SUMSQ) ) / < SUMSQ2*PSUM32 ) 

=  H<4,4) 

=  (  <SUMSQ#PSUM#<XF< 1 )*<XF<2)-XTRAJ( 16) )  + 

XF<4)*XF<5) )  +  ADD3* 

2 , *XF  <  7 ) *PSUM  >  > / ( SUMSQ2#PSUM32 ) 

=  -PSUM/<SUMSG*PSIJM12> 


C#**  NOW  CALCULATE  EQ  3,  K  <  TI )  =  TEMF'K 
C 

CALL  MMUL  <H,F’MINUS,  TEMPI,  WFiKSPCl ,6,9,9) 

CALL  MART < TEMPI , H , TEMP2 , URKSPC2 , 6,9,6) 

CALL  MADD ( TEMP2 , RMAT , TEMP2 ,6,6) 

CALL  MINV<TEMP2,6,DET,WRKVEC1 ,WRKVEC2> 

CALL  MATB ( H , TEMP2 , TEMPK , WRKSPC3 ,9,6,6) 

CALL  MMUL  <  PM  I NIJS ,  TEMPK ,  TEMPK ,  WRKSPC3 ,9,9,6) 

***  NOU  CLACULATE  EQ  4,  XF(TI+)=XFFLUS 

FIRST  FORM  CZM-ZHMI 

ZRES < 1 , 1 )  =  XTRAJ(l)  -  SUMSQ12  +  GAUSS ( 0 ., STD< 1 ) ) 
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ZRES  < 2,1)  =  XTRAJ<2)  -  ADD1/SUMSQ12  +  GAUSS ( 0  .  ,  STD (  2  )  ) 

ZRES (3,1)  =  XTRA J ( 3 )  -ATAN ( XF < 4 ) /XF ( 1 )  )  +  GAUSS ( 0 .  , STD ( 3 ) ) 
ZRES (4,1)  =  XTRA J ( 4 )  +  ATAN ( XF ( 7 ) /PSUM1 2 )  +  GAUSS< 0 . , STD < 4 ) ) 
ZRES (5,1)  =  -<XF( 1 )*XF(5)  -  ( XF  (  2 ) -XTRA J(  16 )  )  *XF ( 4 )  ) /F'SUM 
1  +XTRA J ( 5 )  +  GAUSS ( 0 .  ,  STD ( 5 ) ) 

ZRES (6,1)  =  (XF(8)*PSUM  -  XF ( 7 ) * ( XF ( 1 ) * ( XF ( 2) -XTRAJ ( 16 ) ) )  F 

1  XF(4)*XF(5) )/ 

2  (SUMSG*F'SUM12)  F  GAUSS ( 0 . , STD ( 6) )  F  XTRA J ( A ) 

NOW  FINISH  EQ  4 

CALL  MMUL  <  TEMF’K  , ZRES ,  XFPLUS ,  URNSPC4 ,9,6,1) 

CALL  MADD ( XFPLUS, XFMINUS, XFPLUS, 9, 1 ) 

0 

C  ***  NOW  CALCULATE  EQ  5,  P < TI+ >=PPLUS 
C 

CALL  MMUL (H,F'MINUS, TEMPI, WRKSPC1 ,6,9,9) 

CALL  MMUL  (  TEMF'K  ,  TEMPI ,  PF'LUS  ,  WRKSPC5 ,9,6,9) 

CALL  MSUB  ( PM  INUS ,  PF'LUS  ,  F'F'LUS  ,9,9) 

C 

r;  ***  TAPE6  OUTPUT 
C 

IF  ( IBUG4  .EQ.  1  .AND.  IRUN  .EQ.  1)  THEN 
WRITE ( 6 ,  # ) 

WRITE (  6  ,  #  ) '  H  MATRIX  AT  TIME  =  ',T 
WRITE (6,*) 

DO  220  K-1,M 

WRITE ( 6 , 300 )(K,I,H(K,I),I=1,NF) 

220  CONTINUE 

WRITE <6, *) 

END  IF 

IF  ( I  DUGS  .EQ.  1  .AND.  IRUN  .EQ,  1)  THEN 
WRITE (6,*) 

WRITE (6,*) 'K(TI )  OR  N  MATRIX  AT  TIME  =  ',T 
WRITE  ( 6 ,  #) 

DO  230  K-- 1 ,  NF 

WRITE  (  6 , 3 1 0  )  (  K  ,  I ,  TEMF'K  (  K  ,  I )  ,  1^1  ,M) 

230  CONTINUE 

WRITE <  6 , # ) 

END  IF 

IF  < IBUG6  .EQ.  1  .AND,  IRUN  . EQ »  1)  THEN 
WRITE ( 6 , ♦ ) 

WRITE  ( 6  ,  #  )  'XF ( TI F )  OR  XFPLUS  VECTOR  AT  TIME  =  ' ,T 
WRITE <  6 ,  # ) 

WRITE ( 6, 320 ) <1, XFPLUS (1,1) ,1=1, NF ) 

WRITE ( 6 ,  #) 

WRITE (6,*) 

WRITE  (6,#)  'F'(TIF)  OR  F'PLUS  MATRIX  AT  TIME  =  '  ,T 
WRITE (6,*) 

DO  240  K-l ,NF 

WRITE  (  6 , 300 )  ( K  ,  I , PF'LUS  (  K  ,  I )  ,I-1,NF) 

240  CONTINUE 

WRITE  <  6 , * ) 

END  IF 


n  n  o 


RETURN 

300  FORMAT <9(1 1,11, El  1.5, IX)) 

310  FORMAT (6(11, II, El 1.5, IX)) 

320  FORMAT (9(11, IX, Ell. 5, IX)) 

END 

♦DECK  SAVSTAT 

SUBROUTINE  SAVSTAT 
C 
C 

C  +  +  +  =SAVST AT  =  IS  CALLED  FOR  EVERY  RUN  FOR  EVERY  TIME  VALUE 
C  +  +  +  STORED  ON  THE  EXTERNAL  TRAJECTORY  OUTPUT  OF  HILL5 . 

C  +++  SAVSTAT  HAS  TWO  FUNCTIONS. 

C  ***  FIRST,  IT  SAVES  DATA  TO  BE  POSTPROCESSED  BY  SUBROUTINE 
C  ***  POSTPRC.  SECOND,  IT  CONTROLS  OUTPUT  TO  TAPE6. 

C 

COMMON/INITF'RB/NF  ,NS,M,NXTJ,T  ,TQ,TF  ,DTMEAS,  ISEED,  IF'ASS,  I  RUN 
1  I BUG1 , IBUG2 , I BUG3 , IBUG4 , IBUG5 , IBUG6 , IBUG7 

COMMON/READEXT/XTRA J  <  20 ) 

COMMON/UPDATE/ZRES (6,1), XFPLUS (9,1), PPLUS (9,9) 
COMMON/SAVSTAT/XE ( 9 ) , SUMXE ( 500 ,9,2), SUMPPL ( 500 , 9 ) 

COMMON/ IN I TRUN/ I COUNT 

***  FORM  ST ATISICAL  DATA 

ICOUNT=ICOUNT  +  1 
DO  500  1=1 ,NF 

XE( I )=XTRAJ< I +6) -XFPLUS ( 1,1) 

SUMXE ( I COUNT ,1,1) =SUMXE  <IC0UNT,I,1)  +  XE(I) 

SUMXE  < ICOUNT ,1,2) =SUMXE ( ICOUNT ,1,2)  +  XE(I)**2 
SUMPPL ( I COUNT , I )  =  SUMPPL ( ICOUNT,  I)  +  PPLUS (I, I) 

500  CONTINUE 
C 

C  ***  PRINT  TRUTH  STATES,  ESTIMATED  STATES,  ETC,  FOR  FIRST  IPASB 
C  ***  AND  LAST  IF'ASS  (IF  IBUG1  SET  EQUAL  TO  1). 

C 

IF  ( IBUG7  .EQ.  1)  THEN 

IF  (IRUN  .EQ,  I  .OR.  IRUN  ,EQ .  IF’ASS)  THEN 
WRITE ( 6 ,  ♦  )  '  TRUTH  STATES  XS  AT  T=  '  ,T 
WRITE(6, 1060) ( I ,XTRAJ< 1+6) ,1=1, NS) 

WRITE<6,»)  'FILTER  STATES  XF  AT  T=  ' ,T 
WRITE(6, 1060) ( I , XFPLUS ( 1,1 ) , 1=1 ,NF) 

WRITE < 6  , # )  'ERROR!  ' 

WRITE (6, 1070) (XE( I ) ,1=1 ,NF ) 

WRITE  < 6 , * )  'FILTER  VARIANCE  SUMS' 

WRITE (6, 1070) (SUMPPL (ICOUNT, I) ,1=1, NF) 

WRITE  <  6 , * ) 

WRITE (6,*) 

END  IF 
END  IF 
RETURN 

C1000  FORMAT ( T5 ,  'TRUTH  STATE  ',A4,  '  AT  T  =  ' ,  G12.5) 

C1010  FORMAT ( T5 ,  'FILTER  STATE  ',A4,  '  AT  T  =  ',  G12.5) 

C1050  FORMAT (< 5 < 6X ,  13,  1PG14.6))) 


non  non  non 


tpy^v,,  v\  vy 


1060  FORMAT (9(11, IX, Ell. 5, IX)) 

1070  FORMAT ( 9 ( 2X  ,  E 1 1 .  5 , IX)  ) 

END 

♦  DECK  F'OSTPRC 

SUBROUTINE  POSTPRC 
C 

C  +++  =  F‘0STPRC  =  IS  CALLED  ONCE  AT  THE  ENDING  OF  A  PROBLEM  TO 
C  +++  GENERATE,  SAVE,  AND  PLOT  DATA.  THE  AVERAGE  MEAN  OF  E,  THE 
C  +++  STANDARD  DEVIATION  SE  OF  THE  AVERAGE  MEAN,  AND  THE  PLUS 
C  +++  OR  MINUS  SQUARE  ROOT  OF  THE  DIAGONAL  OF  THE  FILTER 
C  +++  COMPUTED  COVARIANCE  ARE  CALCULATED,  SAVED,  AND  PLOTTED. 

C 

COMMON/SAVSTAT/XE ( 9  > , SUMXE (500,9,2), SUMPPL ( 500 , 9 ) 

COMMON/ INI TPRB/NF , NS , M , NX  T  J , T , TO , TF , DTMEAS , I SEED , I PASS , IRON , 
1  IBUG1 , IBUG2 , IBUG3 , IBUG4 , I BUGS, IBUG6, I BUG 7 

REAL  MNE , MNESQ , FILS IG , FILSAG 
INTEGER  FUNIT 

***  SETUP  OUTPUT  FILES  FOR  ' PLOT  M ' 

OPEN (UNIT=11,FILE=/F’L0TX1' , ST AT US = ' NEW ' ) 
0PEN(UNIT=12,FILE='PL0TX2' , STATUS= ' NEW ' ) 

OPEN (UNIT=13,FILE="PL0TX3' , STATUS= ' NEW ' ) 
0PEN(UNIT=14,FILE='PL0TX4' , STATUS= ' NEW ' ) 

0F'EN(UNIT  =  15.FILE='F'L0TX5' , ST  ATUS= ' NEW ' ) 
0PEN(UNIT=16,FILE=/PL0TX6/ , STATUS= ' NEW ' ) 
OPEN(UNIT=17,FILE='PLOTX7',STATUS=' NEW ' ) 

OPEN ( UNIT1- 18 , FILE  = '  PLGTX8 '  ,  STATUS* '  NEW  '  ) 

OPEN ( UNIT  =  19 , FILE- ' PL0TX9 ' , ST  ATUS= ' NEW ' ) 

%%%  REWIND  ALL  UNITS  ABOVE 

DO  10  I =11 r 19 
REWIND  I 
10  CONTINUE 

***  WRITE  POSTPROCESSOR  DATA  TO  THE  UNITS 

DO  30  I ~1 , TF/DTMEAS 
T=< I*DTMEAS> -DTMEAS 
DO  20  J=1 , NF 

FUNIT  =  J  +  10 
MNE  =SUMXE  ( I ,  J,  1 ) /  IF'ASS 
MNESQ  =SUMXE  <  I ,  J ,  2 )  /IF'ASS 
TRIJSIG=MNE  +  ( MNESQ-MNE**2 )  ** .  5 
TRUSAG=MNE  -  ( MNESQ-MNE**2 ) ** . 5 
FILSIG=SQRT(SUMPF'L(  I ,  J) /IF'ASS) 

FILSAG=-FILSIG 

WRITE < FUNIT, *)T, MNE,  TRUSIG,TRUSAG,FII.SIG, FILSAG 
20  CONTINUE 
30  CONTINUE 
RETURN 
END 


»FR£<INI  T 

QFIN( 1 ) =373250. ,373250. ,373250.  , 

RFIN< 1 )=2500. ,625. , 3 . 00E-04 , 3 . 00E-04 , 4 . 900E-04 , 4 . 900E-04 , 
TAU< 1 )=. 142857143,  .142857143,  .  1  42857.1 43  ,  IPASS=5 ,  IBUG1  =  1  , 1 
TF=15.0,« 

E0I  ENCOUNTERED, 
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IBUG2-1 , IBUG3-1 
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APPENDIX  F 


Filter  Operating  Time 

The  operating  time  of  the  Kalman  filter  is  estimated  by 
calculating  the  number  of  additions,  multiplications,  divides 
and  square  root  functions  for  one  filter  cycle.  Then  the 
function  times  provided  by  OO-ALC  for  the  F-4E/G  fire  control 
computer  are  multiplied  times  the  above  functions.  The 
number  of  functions  are  calculated  by  formulas  (5:403)  and 
then  adding  the  number  of  calculations  required  in  the 
evaluation  of  H  (see  Equation  (3-15)).  The  overall 
calculation  is  a  worst-case  evaluation,  since  in  the 
evaluation  of  H,  many  of  the  relations  are  common  and  need 
only  be  calculated  once  and  then  stored  for  recall.  The 
calculation  results  that  follow  do  not  take  this  into 
account.  The  number  of  functions  required  for  linear  filters 
are  listed  in  Table  F-l.  Then  the  number  of  functions 
required  in  the  evaluation  of  H  are  listed  in  Table  F-2. 

Table  F-3  lists  approximate  filter  cycl  e  times  for  various 
filter  combinations,  calculated  by  adding  entries  from  Tables 
F-l  and  F-2  and  multiplying  by  corresponding  function  times. 


Table  F-l 


Linear  Filter  Update  Operations 


Conventional'*' 

Kalman 

U-D2 

U-D3 

Adds 

2313 

2718 

2448 

Multiplies 

2673 

2997 

2745 

Divides 

6 

62 

44 

Square  Roots 

0 

0 

0 

nine  states 

,  six  measurements, 

and  nine-by-nine 

fa 

-  nine  states 

,  six  measurements, 

and  nine-by-nine 

-  nine  states 

,  four  measurements 

,  and  nine-by-nine 

I  AHcts 

Multiplies 
j  Divides 
|  Square  Roots 


Table  F-2 


Evaluation  of  H  Operations^ 

Six  Measurements  Four  Measurements 

333  51 

209  77 

24  14 

18  12 


|  -  worst  case  design,  many  of  the  relations  are  common  and 

!  can  be  stored  and  recalled,  instead  of  recalculated 


Table  F-3 


Approximate  Operations/Operating  Times 

for  One 

Filter 

Cycle 

Adds 

Multiplies 

Divides 

Square 

Time-*- 

Root  s 

( msec  ) 

Conventional  Kalman 

2446 

2882 

30 

TS 

U-D  (6  measurements) 

2851 

3206 

86 

18 

41.17 

U-D  (4  measurements) 

2501 

2822 

58 

12 

35.21 

computed  as  [( adds )(. 003  )  +  ( mult iplies )(. 00868 )  + 

(divides ) ( .02433 ) 

+  ( square  roots  )  ( 

.150)  ] 
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SOfCPU  DATE  HNO  TIME  -  85/09/30.  19.33.44.  SORE  OflT r  9NP  ZWC  -  85/09/30.  18.47.55.  WPfFB,  DISSPLfl,  SGTEPL  VERSION  2. 


STATE  3,  Q(  1 ) -01 2) -0(3) “373.25,  TAU(1 )-.2,TAU(2-3)-.2,  ALL  MEAS 
APQ-120,  BEAM  ATTACK,  INITIAL  RANGE-40,000. ' ,  UPDATE-. 1,  5  RUNS 


SOFEPL  DATE  ANO  TIME  -  85/09/30.  19.33.14.  SOFE  DATE  RIO  TIME  -  95/09/30.  18.47.55. 


-120,  BERM  ATTACK,  INITIAL  RANGE-40,000. ' ,  UPDATE 


85/09/30.  18.47.55.  WPflTB,  DISSPLfl,  SOrEPL  FUSION  2. 


-120,  BE 


SGfCPl  UhTL  HNu  lIMt  -  bvay/iu 


BS/UVXJ 


,  UPDRTE-.l,  5  RUNS 


11 
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J*Vv % 


Epp 


0  USI  U  J'Ji  U  OS  0*0  0‘OS-  O'COl-O  OSI 

(GNOOllS/I^J)  d0:j«3  AJ.I3CTGA  X 


G-20 


Q ( 2 ) — Q ( 3 ) -373250 . ,  TALK  1  )-.2,TRU(2-3)- 
RTTRCK  ,  INITIAL  RRNGE-40,000 .  '  ,  UPDRTE 


B.33.53.  WPflF B ,  01  S'SPLfl ,  SOfEPL 


TJgJfjS  1  LldSSiQ  '9jt*dM _ '£3'££'3I  'tu/O'./SB  -  3^11  GMJ  3130  3J0S  'lEWii  ‘IQ/^/Se  -  3UII  Olftf  JiPO  TdXjPS 


0(2) -0(3) “373250.,  TALK  1 )-.2,TAU(2-3)-.2,  RLL  METIS 
RTTRCK,  INITIAL  RRNGE-40,000. ' .  UPDRTE-.l,  5  RUNS 


E  5,  Q ( 1 ) -Q ( 2 ) -0 ( 3 ) -373250 . ,  TAU( 1 l-.2,TnU(2-3)-.2,  ALL  MEflS 
120.,  BEAM  ATTACK ,  INITIAL  RflNGE-40,000. ' ,  UPDATE-.  1 ,  5  RUNS 


NPfFB,  OiSSPLfl,  SOPEPL  VERSION 


D  TIME  -  &5/10/U1.  it. 41. 37.  WPflFB,  CliSbPLh,  ;u:'Ltl_  vtrtiluN 


SECONDS i 


I  ML  -  dt>/ 


85/10/01.  lb. 41. V.  WPrtFB,  UlbbPLA,  bOFEPl 


WPrtf'B,  UISSPLR,  SUP L.°L  VlKiIUN 


SOTEPL  OHTE  AND  TIME  -  06/  10/U2.  16.j8.01 .  SOEE  D8TE  HND  TIME  -  85/ 10/02.  16.03.02.  HPflFB,  DlSSPL.fi,  SOf'EPL  VEfolUN  2.2 


Q(2)-Q( 3 1-373250. ,  TALK  1 )-.5,TAU(2-3)-.5,  ALL  MEAS 
ATTACK,  INITIAL  RANGE-40,000.-,  UPDATE-. 1,  5  RUNS 


OISSPlA,  SUFtFL  VEkiiUN  2. 


E  2,  Q( 1 1-0(2 1-0(31-373250. ,  TflU( 1 )-.5,TAU(2-3)-.5,  ALL  MEAS 
120,  BEAM  ATTACK,  INITIAL  RANGE-10,000.',  UPDATE". 1,  3  RUNG 


buFLPL  VLK^ION  2.2 


TIME (SECONDS) 

ure  G. 2 . 1  .d 

E  4,  UTTT-I5 ( 2 ) -Q ( 3 ) -373250 . ,  TflU(  l)-.5,TRU(2-3)-.5,  ALL  MEflS 
120,  BCAu  ATTF1CK,  INIT1DL  RRNGE-40,000.  ‘ ,  UPDATE-.  1,  5  RUNS 


SOFt'PL  ORTE  IINO  TIME  -  85/10/02.  lb. 2b. Oi.  SOFE  OATE  AND  TIME  -  85/10/02.  16.03.02.  MPflFB,  OlbSPLA,  50FEPL  VERSION  2.2 


Q( 1 ) -Q f 2 ) -Q ( 3 ) —373250 . ,  TflUll)-.5,TflU(2-3)-.5,  ALL  MERS 
BERM  ATTRCK,  INITIAL  RFINGE“40,000.  ' ,  UPDATE-. 1,  5  RUNS 


SOFEPL  D8TC  AND  TIME  -  85/10/02.  lb. 28. 01.  SOFT  DATE  RND  TIME  -  85/10/02.  10.05.02.  HPfFB,  OISSPLH,  SUFEPL  VERSION  2.2 
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G-52 
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STATE  8,  Q (1 ) -Q 1 2 ) -0 ( 3 ) -373250 . ,  TflUU  )-.5,TAU(2-3)-.5,  ALL  MEAS 
APQ-120,  BEAM  ATTACK,  INITIAL  RANGE-10,000. UPDATE-. 1 ,  5  RUNS 


-.167,TRU(2-3)-.167,  ALL  MEflS 
000. ' ,  UPDATE-. 1,  5  RUNS 


TflTE  2,  Q ( 1 ) -0 ( 2 ) -0 ( 3 ) -373250 . ,  TAU(l)-.167,TRU(2-3)-. 167,  ALL  MEAS 
tPQ-120,  BEAM  ATTACK,  INITIAL  RANGE-40,000.',  UPDATE-. 1,  5  RUNS 
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OXPL  DATE  HNO  TIMC  -  STIc/itt.  22.33.01.  SOPC  DATE  AND  TIME  -  bS/lC/02.  21.5S.17.  HPfiFB.  OISSPLfl,  SOTCPL  VERSION  2. 


ioftp--  OfltC  AnO'TIMC  -  0— 1C/C2.  5j.73.Cl.  OOTE  DATE  fW  TIME  -  85/10/03.  21.55.17.  MPflFB,  0!55PLfl,  SOTEPL  VETO l ON 


WPPfB,  OlSSPLfl,  GQF-EPL  VERSION  2.2 


SOFEPL  QriTE  AND  TIME  -  db/lU/0«: 


STATE  9,  0 ( 1 ) -0 i 2 ) -0 ( 3 ' -373250 . ,  TAUU  )-.  167 ,TflU 1 2-3 3 - .  167 
APQ-12C,  BEAM  ATTACK ,  INITIAL  RRNGE-40,000. ' ,  UPDATE- . 1  ,  5 


0(21-0(31-373250.,  TALK  1 )-. 143, TflU(2-3)-. 143,  ALL  MCAS 
ATTACK  .  INITIAL  RANGE-40, 000. ' ,  UPDATE- .  1 ,  5  RUNS 


373250 


Ohlt  hnL)  TIME  -  65/10/02.  14.34. 52.  WPHKB,  D1SSFLH ,  buFEPL  VERB  ION  2 


CEAA  ATTACK,  INITIAL  RANGC-40,000 .  ' ,  UPDATE 


0(2)-0(3)-37325U.,  TflU ( 1 )-. 143,TAU(2-3)-. M3,  RLL  MENS 
RTTflCK ,  INI  URL  RANGE- 40, 000.  '  ,  UPDATE-.  1,  5  RUNS 


l)-.143,TAUl2-3)-.143,  ALL  MERS 
0,000. ' ,  UPDATE-. 1,  5  RUNS 


DISbPLR,  SOLPl  VEkblUiM 


0(21-0(31-373250. ,  TAU(  1 )-.  M3,TAU(2-3)-.  143,  ALL  MEAS 
ATTACK  ,  INITIAL  RANGE-40, 000. ' ,  UPDATE- . 1 ,  5  RUNG 


SOFEPL  DATE  HNO  TIME  -  85/10/08.22.56.12.  SOFE  DATE  AMD  TIME  -  85/10/08.22.47.35.  WPflFB,  DISSPLH,  SOFEPL  VERSION  2.2 
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0*002  O'OST  0*001  0*0S  0*0  O'OS-  0*001-  0*0SI€*002- 

(QN003S/I33J)  d0dd3  AII3013A  X 


G-75 


SOFEPL  DATE  AND  TIME  -  85/10/08.  22.56.12.  SOFE  DATE  AND  TIME  -  85/10/08.  22.47.35.  HPHFB,  QISSPLA,  SOFEPL  VERSION  2.2 


o’osi  crooi  cros  cro  o'os-  Q’oot-  crosi- 
(aN003S*QN033S/I33J)  N0IIUd313G3U  X 


G-76 


STATE  3,  Q ( 1 ) — Q ( 2 ) —0 ( 3 J  —373250 . ,  TALK  1 )-. 10,TAU (2-3)-. 10,  ALL  MEAS 
APQ-120,  BEAM  ATTACK  .  INITIAL  RANGE-40, 000. ’  UPDATE- . 1 ,  5  RUNS 
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E  5,  Q 1 1 ) -Q ( 2 ) ~Q ( 3 ) -373250 . ,  TAU(l)-. 10,TAU(2-3)-.10,  ALL  MEAS 
120,  BEAM  ATTACK,  INITIAL  RANGE-40,000.',  UPDATE-. 1,  5  RUNS 
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UNCLASSIFIED 


PRELIHINRRV  KALMAN  FILTER  DESIGN  TO  IHPROVE  SIR  COMMIT  4/S 
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RPG-120,  BEAM  ATTACK,  INITIAL  RANGE-  iC, 000. ’ ,  UPDATE- 


( SECONDS ) 


Bb/ 1 0/ 03 .  1 J .  i 7 . 3b .  Hrrt'd,  iJibbf-'LH,  B(a  LfL  .Lh 


UPnPb,  LiI'd^Plh,  Suf  LP 
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OTTFICK,  INITIF1L  RRNGl-40,000.  ' ,  UPDRTE-.l,  3  RUOO 


149300.,  TflU(l)-.M3,TflU(2-3)-.H3,  ALL  MEA 
1  TlflL  RANGE- 40 .000 .  '  .  UPDATE-.  1.  0  RUNG 
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Currently,  the  F-4E/G  uses  a  Wiener-Hopf  filter  for 
estimating  target  position,  velocity,  and  acceleration  during 
air  combat  maneuvering.  As  implemented,  the  errors  between 
the  actual  target  variables  and  the  estimate  of  these 
variables  are  too  large.  The  purpose  of  this  study  is  to 
evaluate  the  feasibility  of  replacing  the  Wiener-Hopf  filter 
with  a  Kalman  filter  in  order  to  obtain  better  estimates. 

The  evaluation  is  made  by  first  designing  an  appropriate 
preliminary  design  Kalman  filter  and  then  testing  the  design 
through  a  Monte  Carlo  computer  simulation  analysis.  The 
computer  simulation  results  indicate  that  the  Kalman  filter 
is  capable  of  significantly  outperforming  the  Wiener-Hopf 
filter,  and  as  such,  should  be  developed  into  a  final  design. 

The  Kalman  filter  contains  nine  states  (three  relative 
target  position,  three  total  target  velocity,  and  three  total 
target  acceleration  states).  Filter  propagation  is  based  on 
linear  time-invariant  dynamics  primarily  because  of  the 
limited  capabilities  of  the  on-board  aircraft  computer.  The 
linear  dynamics  permits  propagation  by  a  state  transition 
matrix.  Measurement  updates  use  six  measurements  (range, 
range  rate,  azimuth  angle,  elevation  angle,  azimuth  rate,  and 
elevation  rate)  available  on  the  F-4.  Both  continuous  time 
sampled-data  and  discrete-time  sampled-data  designs  are 
included. 
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